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Rule-Based Motion Coordination 
For The Adaptive Suspension Vehicle 
On Ternary-Type Terrain 

S.H. Kwak and R.B. McGhee 

Naval Postgraduate School 
Department of Computer Science (Code CS) 
Monterey, CA 93943, U.S.A. 



ABSTRACT 

This study investigates the utility of rule-based coordination of motion for ternary-type terrain locomotion 
by a hexapod walking machine. The ternary-type terrain considered is composed of permitted areas, 
forbidden areas, and ditch areas. The logic for generating motion coordination is written in Prolog while the 
simulation of the terrain and of the vehicle kinematics, as well as low-level on-board computer functions, 
are written in extended Common Lisp and Flavors. It is found that this approach, which utilizes multiple 
programming paradigms for programming motion coordination logic and simulation objects, results in code 
that is much easier to understand and modify than previous motion coordination programs written in Pascal. 
Thus, the code development effort and time are greatly reduced. The authors believe that both the 
methodology and the motion coordination logic presented in this report possess sufficient merit to justify 
full-scale physical testing in the Adaptive Suspension Vehicle at the Ohio State University. 



1. Introduction 



The Adaptive Suspension Vehicle (AS V) is a large six-legged vehicle designed for outdoor 
operation in rough terrain. Limb motion coordination for the ASV is accomplished by an on-board 
computer network consisting of one PC-AT, eight Intel single-board computers, and two special purpose 
computers [1,2]. The software system is hierarchically organized with a clear distinction being made among 
an individual leg control level, a leg motion coordination level, and a body motion planning level [2,3]. 
Except for the two special purpose computers, the application software for the ASV is currently written 
almost entirely in Pascal. A custom designed real-time operating system, written mainly in PL/M, 
coordinates the functioning of all processes running on the various processors of the vehicle computer. The 
total ASV software system involves somewhat more than 150,000 lines of code [2,4], 

An important feature of the ASV is its omni-directional motion capability [1,2] which gives it the 
general maneuverability characteristics of a helicopter. This behavior is achieved by providing the operator 
with a joystick with three major motion axes for control of vehicle forward velocity, lateral velocity, and 
turning velocity respectively [2]. The vehicle control computer accepts these commands and synthesizes a 
sequence of leg movements to produce the desired body behavior. It is assisted in this task by information 
from an optical terrain scanner which provides a map of terrain elevation in the immediate vicinity of the 
vehicle [5], and by force and position feedback from each leg. 

Until now, nearly all outdoor experiments with the ASV have made use of a tripod gait in which 
legs are used in two sets of overlapping tripods [6,7]. This gait was chosen both for its relative simplicity 
and for its known optimality under high speed straight-line locomotion conditions [7,8,9], However, the 
tripod gait is not well suited to extreme terrain situations in which a significant fraction of the area under a 
given leg may be unsatisfactory for load bearing due to the presence of rocks, holes, obstacles, soft soils, 
etc. In the latter case, simulation experiments [10 ,1 1], and initial indoor testing [12], indicate that on-line 
optimization of leg sequencing should give better results. 

Gaits involving real-time optimization of stability or maneuverability in the presence of terrain 
constraints are often called free gaits to distinguish them from the periodic gaits used by walking machines 
and animals in less difficult circumstances [13,14]. Until now, all free gait studies have been performed 
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based on a binary terrain model which includes two types of terrain objects; i.e., obstacle and non-obstacle 
[10,1 1,15,16] . The size of the individual obstacles is restricted to be comparable to that of the feet of the 
ASV with the further assumption that the obstacles are randomly distributed on the terrain. In this study, 
in addition to the above small obstacles, a ditch obstacle, which is a large and structured obstacle whose size 
is comparable to that of the vehicle, is introduced as a third type. Therefore, the terrain model is ternary 
rather than binary, and contains both randomly distributed small obstacles and large ditch obstacles. 

Though the free gait motion coordinator developed in [16] performs well for binary terrain, in this report, a 
new motion coordinator, called the ’’Ditch Crossing Motion Coordinator” is additionally introduced to 
enhance ditch crossing capability. Thus, two motion coordinators coexist in the program, while, at one 
instance, only one of them is allowed to control the vehicle depending on terrain conditions. If the 
existence of a ditch is detected, the newly introduced motion coordinator takes over control of the vehicle 
until the ASV has crossed the ditch. After crossing the ditch, vehicle control is automatically transferred 
back to the Free Gait Motion Coordinator developed in [16] to handle small obstacles effectively. 

Differing from all ASV experiments using an imperative language (Pascal) to encode stepping 
algorithms, this study uses Prolog, Flavors [18], and Lisp as in [16,17] because of the authors’ belief that 
such a multiple programming paradigm is very well suited to the complex nature of the motion 
coordination problem for the ASV, involving a constant interaction among the on-line optimization logic, 
the vehicle and its internal objects, and the numerical routines. In what follows, Prolog is used to program 
the first part, and Flavors and Lisp are introduced to program the second and the third parts. This division 
of the motion coordination problem is suggested by the physiology of animals which utilize a brain, a 
physical body, and muscles to move themselves. Moreover, like the hierarchical organization existing 
among the these functional parts of animals, the program described in this report is also organized according 
to same type of hierarchy. Specifically, the top level of the program is the motion coordinator written in 
Prolog, while the second level is the Flavor objects which simulate a body, legs, and sensory organs. The 
numerical function routines written in Lisp perform all necessary calculations to move the legs and the 
body analogous to the action of muscles. The resulting code is remarkably easy to understand and modify 
because each programming language naturally provides the right programming paradigm for each division of 
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the program. Moreover, the resulting code is at least an order of magnitude shorter than the corresponding 
Pascal code for same reason. Consequently, this approach significantly reduces development time. 

The remainder of this report first presents definitions used in this report, and a discussion of the 
mathematical-model used to simulate terrain and the ASV vehicle. This is followed by a description of the 
ditch crossing motion coordination rule-set, and the use of Prolog to realize both ditch crossing motion 
coordination and normal motion coordination. The report concludes with a discussion of the results of the 
investigation and suggestions for future research. 



2. Definitions 

In this report, a number of definitions are used as follows: 

Definition 1: A foothold is a point on a segment of terrain, and can be assigned to a leg while the 
leg is in the air. When the foot of a leg is placed on the terrain, its assigned foothold becomes the support 
point of the leg. A foothold associated with a leg can be changed to a new one before the foothold becomes 
a support point [10]. 

Definition 2: The support pattern associated with a given set of leg support points is the convex 
hull of the vertical projections of all support points into a horizontal plane [13]. 

Definition 3: The magnitude of the stability margin at time t for an arbitrary support pattern is 
equal to the shortest distance from the vertical projection of the vehicle center of gravity to any point on the 
boundary of the support pattern. If the pattern is statically stable, the stability margin is positive. 
Otherwise, it is not defined [15]. 
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Definition 4: A working volume is associated with each leg. This volume is a subset of three- 
dimensional space defined relative to the body and consists of the collection of points which can be reached 
by the foot of the given leg [11,19]. 

Definition 5: A temporal kinematic margin is associated with each foothold. At any instant, this 
margin is the time remaining until the associated leg would reach the boundary of its working volume if the 
foothold were used as a support point [15,19]. 



3. Vehicle and Terrain Model 

While the vehicle model used in this study is based on the ASV, it represents only the major 
vehicle dimensions and components. Specifically, the cabin and the terrain scanner are omitted from the 
simulation model, while the geometries of the body and the legs are identical to those of the ASV. 
Therefore, the simulation model is represented by a simple six-faced box with each leg drawn as two line 
segments as shown in Figure 1. The exact vehicle dimensional data can be found in other literature [2,7]. 
Differing from the most previous simulation studies related to gaits and control of stepping 
[10,11,15,16,19], which simply ignore the overlapped portions of adjacent working volumes, in this study, 
the overlapped portions are taken into account during the foothold selection process in order to utilize the 
full kinematic capability of the vehicle. 

The terrain adopted for this study is made up of terrain cells, and these individual cells are classified 
into two types of cells. One type, called a permitted cell, is able to support the body load when a leg steps 
on it. The other type, named a forbidden cell, is not usable because of unfavorable terrain conditions. 
Though this classification is complete with respect to individual terrain cells, there is a chance that a group 
of the forbidden cells can constitute a large structured obstacle instead of being randomly distributed. In this 
study, one type of structured large obstacle, a ditch , is considered because of its special shape, a long length 
and a relatively narrow width. Due to its shape, the most effective way to overcome a ditch is, if possible, 
crossing it instead of avoiding it by going around it. However, avoidance may be a better choice for other 
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types of large obstacles. This possibility is not studied in this report Rather, the simulation terrain is 
ternary terrain which is composed of permitted cells .forbidden cells, and a ditch. A typical terrain example 
utilized in this study is shown in Figure 1. A cell with an "X" mark is a forbidden cell or a part of a ditch 
area while unmarked cells are permitted. A ditch is shown in the middle of the simulation terrain. 

Forbidden cells on this terrain can be designated either manually by an operator or automatically by using a 
random number generator with a threshold chosen to produce a specified ratio between permitted ceils and 
forbidden cells [11]. 

The dimensions of each cell are one foot by one foot. This size is comparable to that of the feet of 
the ASV, and is larger than the resolution of the terrain scanner [5,12]. 

An overall block diagram of the program developed in this study is shown in Figure 2. This entire 
program is written for a Symbolics 3650 Lisp machine [1 1,20,21]. Each box shown is an object that is an 
instance of a Flavor [18] with the exception of the Free Gait Coordinator which is written in Symbolics 
Prolog [22]. Like the physical ASV which has nine major parts, namely, a body, a vision sensor, a cab, 
and six legs, the simulation object, "ASV” has correspondingly nine component objects, "Body", Vision 
Sensor", "Joystick", and "Legl" through "Leg6". These nine objects are linked to "ASV" through a part 
relation [16,23]. Each part has its sub-parts, and again is linked to them with a part relation. Differing 
from the nine major parts which have visible corresponding parts in the real ASV, the subparts of the 
simulation are not physically tangible, but are introduced because of their functionalities for program 
development. For example, the "Legl" object, which is a part of the "ASV" object, has six subparts: 

"Legl Plan Machine", "Legl Control Machine", "Legl Executor", "Legl Contact Sensor", "Legl Foothold 
Finder", and "Legl TKM Calculator". Through the use of a make-instance function in an appropriate 
Flavor slot, the "Legl" object binds all of these subparts into one group with the part relation [18]. In 
order to show the above relations among the objects in Figure 2, the six subpart objects are drawn under the 
"Legl" object. 

Besides the part relation. Figure 2 also shows the hierarchical control structure linking the 
simulation objects. Specifically, communication is restricted between objects in two adjacent levels by an 
assumption that upper levels have the right to access status information at lower levels, but the latter must 
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receive explicit commands from upper levels to update their internal states. For example, when "ASV", the 
vehicle object, needs "Legl" to support its body, it sends a "Place" decision to "Legl" and continuously 
monitors "Legl" as to whether "Legl" has begun to support the body or is in motion to try to reach a 
foothold. On receiving a "Place" decision from "ASV", "Legl" sends the "Place" decisions to "Legl Plan 
Machine" while making observations of this machine. This type of message passing to and status 
observation from subordinates continues until the "Place" decision is accomplished. That is, when the foot 
of "Legl" actually hits the ground, the contact sensor of "Legl" detects the event and changes its internal 
state. The state change of "Legl Contact Sensor" is observed by "Legl Executor" and by "Legl Control 
Machine". In this way, the state change in the lowest level is propagated to higher levels until the touch 
down event arrives at "ASV". The detailed description of this control scheme can be found in other 
literature [16]. 

The joystick object simulates the physical three-axis joystick of the ASV through the use of six 
keys on the simulation computer keyboard to increment or decrement each of the three rates controlled by 
the joystick. These rates ar z forward velocity , lateral velocity , and turn rate , all in body coordinates. The 
altitude of the vehicle above the terrain and its orientation in roll and pitch relative to the terrain are 
automatically regulated using the algorithms described in [24]. 

While an elementary representation of the vision sensor is included in the program, as described in 
the above discussion of terrain, it is assumed that all forbidden cells and ditches have already been identified 
by prior terrain analysis. Of course this assumption does not represent a physical limitation of the ASV, 
but as made merely to allow this simulation to be focused on vehicle control, rather than on vision. 

In addition to simplification of vision, this simulation also ignores leg mass in order to avoid the 
complexity of computing a center of gravity which moves with respect to the body. Moreover, all inertial 
forces are omitted from the simulation. That is, as in most previous simulation studies relating to gaits 
and control of stepping [8,9,10,1 1,16,19,25*26], only static stability is considered in this study. While 
this simplification would be serious in high speed locomotion, free gaits are most appropriate to low speed 
traversal of extremely difficult terrain, so the authors do not feel that this is a serious limitation on the 
applicability of the results of this investigation. 
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4. Ditch Crossing Motion Coordination 

When the vision system detects the existence of a ditch, the vehicle operation mode is switched 
from the normal free gait mode [1 1,16] to the ditch crossing mode. In contrast to the normal free gait mode 
which performs on-line optimization of leg stepping under an environment with randomly distributed small 
obstacles [1 1 ,16], the coordinator in the ditch crossing mode controls the vehicle with a predetermined 
motion sequence in order to effectively overcome a ditch; i.e., a structured large obstacle. The ditch 
crossing mode is composed of two phases, the preparation phase and the main phase, and these two phases 
are sequentially executed. The preparation phase provides a transition period from the normal free gait mode 
to the ditch crossing mode, and the main phase performs the actual ditch crossing action. 

4.1 Preparation Phase 

The preparation phase is composed of a sequence of nine states, and these nine states are grouped 
into two cycles which are named Cycle 1 and Cycle 2. The first cycle, Cycle 1, consists of six states and 
takes care of a transition from the normal mode to the ditch crossing mode. During the execution of the 
first cycle, the body attitude is modified to be suitable to cross a ditch. The second cycle, Cycle 2, consists 
of three states and causes the vehicle to have the correct leg configuration for the ditch crossing operation in 
the main phase. Thus, during the execution of Cycle 2, the body is not moved at all. The graphical 
representation of the preparation phase is shown in Figure 3. 

The six states in Cycle 1 are Place Legs in the Air , Back Middle Legs , Forward Rear Legs , 
Forward Middle Legs , Forward F ront Legs , and Lift Middle Legs and Move . Body movement is involved 
only in the last state. The first state, Place Legs in the Air , represents a simple action; i.e., leg placing, 
but the rest of the states represent at least two sequential actions. For example, during the Forward Middle 
Legs state, the middle legs are lifted from the ground and placed on the ground using one of the closest 
footholds to the front end of the working volumes of the middle legs. Similarly, in the Back Middle Legs 
state, the middle legs are lifted and placed at one of the closest footholds to the back end of the working 
volumes of the middle legs. 
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The first cycle of the preparation phase begins with the Place Legs in the Air state. During this 
state, all the legs in the air are placed on the ground without any body movement. At most three legs will 
be placed in this state because at least three legs must support the body at all times to maintain the stability 
of the vehicle. When a leg is placed in this state, one of the closest footholds to the front end of the 
working volume of each leg is selected as a stepping position on the ground. Thus, the newly placed legs 
have larger temporal kinematic margins (TKMs) than the legs already on the ground. 

At the end of the first state, all six legs are on the ground. Thus, the middle legs can be used for 
any purpose because the front and the rear four legs are sufficient to make the vehicle stable as long as these 
four legs are within their kinematic limits. Therefore, the middle legs are used to provide maximum TKMs 
for the front and the rear legs in the following four states. 

The Back Middle Legs state is the first state of the sequence to maximize TKMs of the front and 
the rear legs. The middle legs are lifted and placed at the back ends of their working volumes. At the 
completion of this state, the middle legs are placed behind the center of the gravity of the vehicle. Thus, 
the vehicle can maintain its stability with the front and the middle legs alone, and the rear legs can be lifted. 

In the Forward Rear Legs state, the rear legs are lifted and placed at the front end of the working 
volume of the rear legs. Thus, both rear legs will have maximum TKMs at the end of this state. Though 
the rear leg lifting actions are inherently safe, before lifting one of the rear legs the vehicle stability is 
checked to ensure that the vehicle is stable without the rear leg. If the vehicle is not stable, the leg is not 
lifted from the ground, and the ditch crossing operation will be halted. If it is stable without the leg, the 
leg is lifted. After the first rear leg is lifted safely, the same test is performed on the other rear leg before it 
is lifted. This type of test is always performed before lifting any leg from the ground during the ditch 
crossing operation in order to ensure safe operation. At the end of the Forward Rear Legs state, both of the 
rear legs are placed. Again, the middle legs become redundant for the vehicle stability. 

In the Forward Middle Legs state, the middle legs are lifted and placed at the front end of the 
working volumes of the middle legs. Because the new support points of the middle legs are ahead of the 
center of the gravity of the vehicle, the vehicle now can be safely supported by the middle and the rear legs. 
Thus, the front legs can be lifted from the ground without harming the stability of the vehicle. 
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In the Forward Front Legs state, the front legs are lifted and placed at the edge of the vehicle side of 
the ditch. This has to be done because the new support points of the front legs will be the last ones on the 
vehicle side of the ditch. This requirement is not hard to meet since, as long as the edge of the ditch is 
included in the working volume of the front legs, the front legs can always be put in the right position. 
However, the opposite side of the edge of the ditch should not be included in the working volume in order 
to prevent its being used as possible footholds for the front legs. Therefore, there is a range of the vehicle 
locations with respect to the near edge of the ditch so that the vehicle can select the right stepping positions 



for the front legs. This range is shown in the following equation: 



1 

P + yL-DW< DCIR < 




( 1 ) 



where DCIR 

P 

L 

DW 



: Ditch Crossing Initiation Range 
with respect to gravity center of the vehicle 
: Pitch between adjacent legs 
: Longitudinal length of 
working volume for each legs 
: Ditch Width. 



To understand this relationship, it should be recognized that the distance from the vehicle's center to the 
front ends of the working volumes of the front legs is the sum of the pitch between the middle and the front 
legs and half of the longitudinal length of the working volume of the front legs. Thus, the meaning of 
Eq. (1) is that when the ditch crossing operation is initiated, the front ends of the working volumes of the 
front legs should be positioned between the near edge and the far edge of the ditch. Evidently, if ditch 
crossing is initiated anywhere in the above range, the ditch crossing operation will not be hampered since 
the body attitude and the leg stepping positions are corrected during the preparation phase. It should be also 
noted, however, that if DW is less than 3 ft, the vision system does not have to detect the existence of the 
ditch at all. The normal plan developed in [16] is capable of handling such ditch width without any 
problem. 

At the completion of the Forward Front Legs state, the front legs will be placed on the vehicle 
side edge of the ditch. Again, the middle legs become redundant. 
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In the Lift Middle Legs and Move state, the middle legs are lifted and the body is moved into the 
ditch area until at least one of the supporting legs (the front and the rear legs) reaches its kinematic limit. If 
too many obstacles have not interfered with the operations of the previous four states, there will be a high 
probability for the front legs to reach their kinematic limits first because footholds in the front most 
portion of the working volumes of the front legs may be excluded by the location of the ditch. Thus, at the 
end of this state, the vehicle body is fully pushed into the ditch area under the constraints of the current leg 
configuration, and its movement is stopped. At this point, although the body position and the front leg 
positions are right for the ditch crossing, the rear leg positions are not appropriate because they are already 
near their kinematic limits. Thus, further body movement is very limited or impossib# depending on the 
kinematic margins of the rear legs. Even though the opposite side of the edge may not be reachable by the 
front legs at the end of the Lift Middle Legs and Move state, the vehicle can cross the ditch if it moves 
further into the ditch area by eliminating the kinematic problem of the rear legs and if the ditch width is 
narrower than the vehicle ditch crossing capability. In the second cycle of the preparation phase, the 
kinematic limits of the rear legs are eliminated. 

The second cycle of the preparation phase has three states and starts with the Back Middle Legs 
state. No body movement is involved in the second cycle, but the stepping positions of the rear and the 
middle legs are rearranged. 

In the first state, the middle legs, which are redundant to make the vehicle stable, are lifted and 
placed near the back end of their working volumes. Because the new support points of the middle legs are 
behind the center of the gravity of the vehicle, the rear legs can be lifted without harming the vehicle 
stability. 

In the Forward Rear Legs state, the rear legs are lifted and placed near the front end of their worki ig 
volumes. Thus, the rear legs obtain maximum kinematic margins. At this point, though the rear legs 
provide maximum body movement potential, the middle legs prohibit further body movement. 

In the Forward Middle Legs state, the middle leg kinematic problem is eliminated. The middle 
legs are lifted and placed at the front end of the working volume of the middle legs. Therefore, both the 
middle and the rear legs have their maximum kinematic margins, while the body is completely pushed into 
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the ditch area. In contrast to the middle and the rear legs, the front legs should be at their kinematic limits 
because the front legs stepping positions have not changed since they were at their kinematic limits at the 
end of Cycle 1. If the front legs are now lifted, vehicle body movement can be resumed. This will be the 
first action of the following phase, the Main Phase. 

In summary, at the end of the preparation phase, the body is fully moved into the ditch area within 
the limits of the stability of the vehicle and the kinematics of the legs. The middle legs and the rear legs 
are fully forward to enhance the ditch crossing capability, and the front legs are ready to be lifted from the 
ground. This preparation allows the vehicle to cross a wider ditch than the longitudinal length of the 
vehicle legs’ working volumes. Though the other side of the ditch is not included in the front legs’ working 
volumes at the end of the preparation phase, the body can move forward as long as the vehicle's stability is 
maintained and the leg kinematic limits are not reached. That is, if the other side of the ditch is included in 
the front leg's working volume before the vehicle becomes unstable and before the other legs reach their 
kinematic limits, the other side is reachable by the front legs. If the front legs can be placed on the other 
side within the vehicle’s kinematic and stability limitations, then the vehicle can cross the ditch because the 
AS V legs’ working volumes are identical and because the pitches between the front legs and the middle legs 
and between the middle legs and the rear legs are the same. 

As a result of the above arguments, the maximum ditch width can be crossed by the ASV, which 
has identical working volumes for all legs and the equal pitches between the front and the middle legs and 
between the middle and the rear legs, is determined by both the pitch length and the length of the 
longitudinal working volumes of the legs. Specifically, the maximum ditch width can be crossed by the 
ASV is given by: 

MDW = P + j L - SM - SDE (2) 



MDW 


: Maximum Ditch Width 


P 


: Pitch between adjacent legs 


L 


: Longitudinal length of 




working volume 


SM 


: Safety Margin 


SDE 


: Search Digitization Effect. 
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As can be seen, the maximum ditch width (MDW) is calculated by adding the pitch between adjacent legs 
and the half of the longitudinal length of the working volumes of the legs, and then by subtracting the 
safety margin (SM) and the search digitization effect (SDE). The safety margin is a prescribed margin 
ensuring safe operation of the vehicle. The search digitization effect is an artifact of the foothold search 
process resulting from a one foot by one foot grid search. With the dimensions of the simulation model 
discussed in [2,7], the MDW becomes 8.5 ft when the SM and the SDE are 0.5 ft, respectively. 

4.2 Main Phase 

The Main Phase is composed of three cycles The first and the third cycles are composed of three 
states each, while the second cycle contains only one state. The first cycle in the main phase in the 
program is named Cycle 3 to show continuation from the preparation cycles. Consequently, Cycle 1 and 
Cycle 2 belong to the Preparation Phase , and Cycle 3 through Cycle 5 belong to the Main Phase. A 
graphical representation of the Main Phase is shown in Figure 4. 

The first cycle, Cycle 3 is composed of three states, Move Forward Front Legs , Move Back Middle 
Legs , and Move Forward Rear Legs. During this cycle, the front legs cross the ditch, and the rear legs are 
prepared to replace the middle legs which will cross the ditch in the following cycle. In this cycle, the 
vehicle body is allowed to move forward whenever possible. Therefore, all the state names are pre-fixed 
with "Move", and each state is composed of three actions, leg lifting, body movement, and leg placement. 

In the Move Forward Front Legs state, first, the front legs are lifted while the body is not moved. 
As soon as both front legs are lifted from the ground, the second action is performed, which is a forward 
body movement. This body movement is sustained until the middle legs limit this movement because the 
middle leg positions with respect to the center of the vehicle gravity determine the stability margin when 
only the middle and the rear legs support the body. Though the middle legs can kinematically move behind 
the center of gravity, they should be stopped in front of the center of gravity to maintain the safety stability 
margin. 

When the body movement is stopped with the completion of the second action of the current state, 
the opposite side of the ditch will be included in the working volumes of the front legs if the width of the 
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ditch is narrower than MDW. Thus, the third action of the Move Forward Front Legs state follows, in 
which the front legs are placed on the opposite side of the ditch. Thus, the middle legs become redundant 
for the vehicle stability. 

In the second state of Cycle 3 , the Move Back Middle Legs state, the middle legs are lifted from 
the ground. The vehicle body movement is resumed because the movement is restricted by the middle legs 
to maintain the vehicle stability margin. The body movement is continued until any one of the supporting 
legs meets its kinematic limit. Specifically, the body will be moved until one or both rear legs reach its or 
their kinematic limits because the kinematic margins of the rear legs have been used to move the body in 
the previous state, but those of the front legs have been just maximized in the previous state, the Move 
Forward Front Legs state of Cycle 3. This effect can be easily seen in the second drawing of Cycle 3 .‘State 
2 in Figure 4, and "Rear Legs" written on the top of the second drawing shows the termination condition of 
the current body movement. When the kinematic limits of the rear legs stop the body movement, the third 
action of the current state is performed, which is to place the middle legs at the back end of their working 
volumes. Because the middle legs are placed behind the center of gravity, the rear legs can be lifted from the 
ground while the front and the middle legs stably support the body. 

In the Move Forward Rear Legs state, which is the third state of Cycle3 , first, the rear legs are 
lifted from the ground. As soon as the rear legs are lifted, the body movement is resumed. However, the 
body movement is immediately blocked by the middle legs which have placed back in the previous state. 
Thus, the third action of the current state, which places the rear legs at the front end of their working 
volumes, is immediately started. Consequently, very little body movement is involved in this state, but 
the rear legs gain large TKMs so that the body can be moved further in the next cycle. Therefore, though 
the other side of the ditch may not reachable by the middle legs under the current body position, the new 
body movement, which will be performed in the next cycle, will make this possible. 

The second cycle of the main phase, Cycle 4, is composed of one state, the Move Forward Middle 
Legs state. In this cycle, the middle legs will be moved to the other side of the ditch. The first action is 
lifting the middle legs so that the body movement can be resumed. This movement will last as long as the 
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front legs have positive TKMs because the TKMs of the front legs have already been partially consumed in 
the previous cycle. This is shown in the second drawing of Cycle 4 :State 1 in Figure 4. 

When the body movement is stopped, the other side of the ditch is reachable by the middle legs 
because the geometries of the front and the middle legs are identical and because the working volumes of the 
front and the middle legs overlap slightly at the rear end of the former volume and the front end of the latter 
volume. As soon as the middle legs are positioned on the other side of the ditch, this cycle is terminated. 

The last cycle, Cycle 5, which is the third cycle of the main phase, takes care of the ditch crossing 
action of the rear legs. This cycle is composed of the three states, the Move Forward Front Legs state, the 
Move Back Middle Legs state, and the Move Forward Rear Legs state. These three states are the same that 
of Cycle 3. This is not a coincidence, but an expected consequence of the geometrical symmetry of the 
front and the rear legs. 

In the Move Forward Front Legs states, the kinematic problems of the front legs which block 
further body movement are relieved because the first action of this state is to lift the front legs from the 
ground. Thus, the body movement, which is the second action of this state, is resumed, and is terminated 
by the positions of the middle legs with respect to the body because the positions of the middle legs 
determine the stability of the vehicle. When this state is terminated, the front legs are placed on the ground. 

In the Move Back Middle Legs state, the body movement is resumed as soon as the middle legs are 
lifted from the ground. This movement will last until the kinematic limits of the rear legs are reached. 
When this condition is met, the body movement is stopped and the middle legs are placed as far backward as 
possible so that the middle legs can support the body together with the front legs. Consequently, the 
middle legs will be placed at the edge of the ditch because this edge is in the working volumes of the middle 
legs. 

The Move Forward Rear Legs state, which is the last state in the last cycle, causes the rear legs to 
cross the ditch. The first action is to lift the rear legs from the ground. Body movement is then resumed 
and continued until the other side of the ditch is reachable by the rear legs. Again, this will be 
accomplished because the working volumes of the middle and the rear legs slightly overlap. Finally, all the 
legs are across the ditch. Thus, the ditch crossing operation has accomplished and the operational mode is 
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switched back to the normal mode, and the Free Gait Motion Coordinator [16] regains control of the 



vehicle. 



5. Program Implementation 

The top level Ternary Terrain Motion Coordinator is written in Symbolics Prolog because of its 
easy translation characteristics from natural language to a computer program, and because of its 
straightforward interface to Symbolics Lisp language in which the rest of the program is written. The 
Prolog program is listed in Figure 5. It is composed of three functional groups of predicates. The first 
group controls the flow of the whole program, while the second does logic processing which generates 
commands for the vehicle body and legs. The last group is responsible for bridging between the program 
written in Prolog and the robot program in Flavor objects. This is accomplished through the Lisp function 
call facility provided by Symbolics Prolog. Specifically, anything following the "is” predicate in a Prolog 
clause may be either a Prolog arithmetic function or the name of a Lisp function [22]. If a Lisp function 
name follows the "is” predicate, it is evaluated according to its definition inside the Lisp environment. In 
the program of Figure 5, arguments following "is” predicates are names of Lisp functions, and make 
connections to the Lisp environment. A returned value resulting from a Lisp function call may be used to 
instantiate a variable preceding the "is" Prolog predicate or test whether the returned value matches a value 
preceding the "is” predicate. In the former case, the subgoal "is" always succeeds, but the latter case, only 
when two values agree does the "is" subgoal succeed. In the program, only the former case is used. The 
Lisp portion of the program is listed in the appendix attached at the back of this report. 

The Prolog program is started by typing "robot" on the computer console. The robot clause is the 
first line of the program. After the initialization process is done, it makes the loop clause repeat. Thus, it 
determines the flow of the whole program. 

The loop clause is composed of three subgoals, get_command, plan, and execute. This shows the 
flow of the program execution for each loop. Based on the input command from the joystick, motion is 
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planned, and the planned motion is executed. Then, the executed motion is drawn on the screen by the 
drawjobot clause which actually calls a corresponding Lisp function, graphical _display. 

The plan subgoal of the loop clause has two alternatives, free_gaitsjnotionjcoordination _plan and 
ditchjcrossingjnotionjcoordination _plan. In the Prolog program, the ditch jnode subgoal is tested first 
because ditch j:rossingjnotionjcoordination _plan deals with a more specific case than the other. If the 
ditch jnode subgoal succeeds, then ditch jcrossingjnotionjcoordination jclan is executed. If not, then 
freejgaitsjnotionjcoordination j>lan is executed. The free_gaits_motion_coordination j)lan clause is 
composed of update jobotjstate, checkjkmjimit , leg j)lan, body j)lan, and generate ^decision subgoals. 
The first and the second subgoals update the state and the body position of the vehicle check kinematic 
problems of the legs. The third subgoal, leg _plan , performs on-line optimization for leg coordination 
using the free gait strategy [11,16]. Based on the leg plan, the fourth subgoal, body _plan , plans the body 
movement to enhance the vehicle stability. Finally, the generate jdecision, subgoal sends decisions to the 
"AS V" robot flavor object. A detailed description of the free_gaitsjnotionjcoordination j>lan can be found 
in other literature [11,16]. 

The ditch jcrossingjnotionjcoordination J>lan and the related clauses implement the ditch crossing 
coordination discussed in the previous section. There are two ditch jcrossingjnotionjcoordination jclan 
clauses in the program, and the first clause checks the termination condition while the second clause 
performs the ditch crossing planning. If the ditch crossing activity is not completed, then the first clause 
fails, and the second ditch jrossingjnotionjcoordination jplan clause is executed. Thus, the cycle _planner 
clause is called into an action. 

The cycle _planner clause and related clauses follow the above two ditch jclan clauses. This group 
of clauses is named "Cycle Planner". The first clause of the "Cycle Planner" group, ditch jplanjione , 
checks the completion of the ditch crossing plan. The two cycle jclanner clauses take care of ditch plan 
cycle changes from cycle 1 to cycle 5 by increasing the cycle number whenever one cycle is completed. 
Therefore, the ditch jylcmjlone clause succeeds as soon as cycle 5 is finished because the cycle number 
becomes 6 immediately after the completion of Cycle 5. The last subgoal of ditch j)lanjdone y is 
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idlejoycle , which is a dummy plan without any leg planning. It is introduced to fill the gap for the 
transition between cycles. 

The subgoals used in the cycle j>lanner clauses, onejyclejlone and planjcycle , are grouped in the 
following part of the program, which is called the "Plan Cycle Dispatcher" group. The onejcyclejione 
clause checks the termination condition of one plan cycle, and the five planjycle clauses execute the 
appropriate ditch plan cycles based on the plan cycle number which is given through the planjycle fact in 
the Prolog data base. 

The ditch jlanjycle clauses, ditch jlanjyclel through ditch jlanjycleS, form another group 
called "Cycles" in the program following the "Plan Cycle Dispatcher" group. The first subgroup, 
ditch jlanjyclel , has seven clauses. The first clause of this subgroup takes care of the initial state 
transition, and the rest of them represent the six states in cycle 1 discussed in the previous section. 
Specifically, the first clause retracts plan state( start), which is a cycle starting fact, from the Prolog data base 
and asserts a new fact, planjtate(place_legs_in_the_air ), which is the name of the first state. After changing 
the Prolog data base, the first clause executes the place JegsJnjhejir subgoal, which performs the place 
legs in the air state in Cycle 1. When the place JegsJnjhejir subgoal is executed, the first clause 
provides the next state information for the subgoal so that when the current subgoal is completed the correct 
information about the succeeding state is asserted in the data base. The second clause through the seventh 
clause sequentially represent six states in cycle 1. Thus, these ordered clauses represent the sequence of state 
transitions among the six states. When the last clause calls the liftjniddleJegsjndjnove subgoal, 
one jlanjyclejdone is given instead of the name of the next state to assert the cycle termination fact in the 
data base. This structure is repeated for the rest subgroups of the "Cycles" group, ditch jlanjycle2 through 
ditch jlanjycle5. 

The following group called "States" is composed of 10 different subgroups, and each subgroup is 
composed of two clauses. These clauses accept information about the next state so that the next state 
information is asserted when the current state is completed. However, only the first clause, which takes 
care of its state transition, utilizes the next state information. The second clause ignores the next state 
information and executes a subgoal whose name is its clause head name pre-fixed with "do_". Additionally, 
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both clauses of each subgroup determine the body movement by executing either the stop or the move 
subgoal depending on the needs of each state described in the previous section. 

The "State Executors" clause group follows the "States" clause group. This clause group is 
composed of 11 subgroups of clauses. Among them, 10 subgroups are responsible for execution of 10 
states in the "States" group, while the eleventh subgroup takes care of the body movement, such as move , 
slop , clear jnove jnemory , and movejdone. Therefore, except for the last subgroup, each subgroup shows a 
sequence of actions within a state, which are described in the previous section. For example, the 
do_backjniddleJegs clause subgroup, which is the first subgroup of the "State Executors" group, is started 
with three major clauses. The first clause, backjniddleJegsjione, tests the state termination condition, and 
the second and the third clauses perform a sequence of actions, which are lifting the middle legs and then 
placing them at the back side of their reachable areas. The second clause tests whether both middle legs are 
lifted by executing the subgoal, alljniddlejegsjifted. Initially, this test should fail. Thus, the third clause 
is executed. After the third clause is executed twice, the both middle legs will have been lifted from the 
ground because the liftjniddlejegs subgoal causes one middle leg to be lifted from the ground at a time. In 
the dojbackjniddlejegs subgroup, there are two liftjniddlejegs clauses. The first clause performs the leg 
lifting action by selecting one middle leg and then causing it to be lifted from the ground, while the second 
clause performs a default action by always succeeding. Only when the both middle legs are lifted from the 
ground, is the middle leg placement executed. Specifically, when the alljniddlejegsjifted subgoal in the 
second clause of this subgroup is satisfied, the place jniddlejegsjtack subgoal is executed. If the middle 
legs are placed on the ground again, then the backjniddleJegsjione clause, which is the first clause of the 
current subgroup, succeeds because the alljniddlejegsjifted subgoal has been satisfied by the 
middle Jegs(lif ted) fact which was asserted when middle legs were lifted from the ground, and because the 
alljniddlejegs jplaced subgoal is now satisfied. Before completing the backjniddleJegsjione clause, the 
first clause of the current subgroup, the clear jniddlejif ted jnemory and the clear jnove jnemory subgoals 
clear residual facts generated during execution of the dojbackjniddlejegs clauses in order not to interfere 
with the execution of the following "State Executions" subgroup clauses. 
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Rest of the subgroups in the "State Executors" group have the exactly same structure that of the 
do_back_middleJegs clause subgroup. Specifically, one state termination clause is followed by two state 
execution clauses which are pre-fixed with "do_" and related clauses which support these leading three 
clauses. If the related clauses are already available, they are not duplicated by adding them in the subgroup. 

The only exception to the above structure is the fifth subgroup, the dojiftjniddlejegs clause 
subgroup. This subgroup is composed of one clause, and there is no clause to test the state termination 
condition. The time required to complete the middle leg lifting action in the lift_middleJegs_andjnove 
state, which is the only state that utilizes the dojiftjniddlejegs clause, is considerably shorter than that 
needed to complete the body movement in the state. Thus, the leg lifting action is always guaranteed before 
the current state is terminated. 

The last group of clauses is named "Plan Libraries". These clauses are used by both 
ditch jcrossing_motion_coor dination j?lan and free _gaitsjnotion -Coordination j?lan. This group is 
composed of two subgroups, body _plan and generate -decision. The latter subgroup sends planned leg 
motions through decisions to the robot, "ASV", which is a flavor object. It sends them one by one until 
all the decisions in the Prolog data base are exhausted. The former subgroup takes care of body movement 
by executing speed j?lan and trajectory j>lan. The speed j?lan clauses control the speed of body movement 
and the trajectory j?lan clauses modify body movement trajectory in order to increase the stability margin of 
the vehicle using a "push" operation which causes the gravity center of the vehicle to move away from the 
boundary of the current supporting pattern [16]. 



6. Discussion 

Performance tests were carried out for various terrain conditions by making the ASV follow a 
prescribed standard trajectory. The standard trajectory is a straight line across the model terrain from one 
side to the other side while crossing a ditch oriented perpendicular to the direction of vehicle motion. No 
failures to complete the standard trajectory were observed for any terrain containing up to a 8 ft width ditch 
if no randomly distributed forbidden cells were included in the terrain. However, when forbidden cells were 
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added to the terrain with a 8 ft width ditch, the performance was severely degraded. If the randomly 
distributed forbidden cells occupied 30 percent or more of the area of the non-ditch portion of the whole 
simulation terrain, the ASV always failed to complete the standard trajectory. Specifically, the ditch 
crossing operation was halted because the random obstacles on the ground prevented the ASV from using 
the most favorable stepping positions near the ditch. However, when the width of a ditch was reduced to 
7 ft, no failures in ditch crossing operations were observed. Rather, the capability to overcome randomly 
distributed forbidden cells became the bottle neck which determined whether the ASV could complete the 
standard trajectory or noL Overall, when less than 70 percent of the total terrain cells were the forbidden 
cells, the program made the ASV follow the standard trajectory without great difficulties. 

One of the advantages of using object-oriented programming for the ASV object and its subobjects 
was the easy extension to a new ASV with additional functionality required for the ditch crossing 
maneuvers. Specifically, this was accomplished by using the inheritance mechanism provided by Flavors. 
The original ASV was an instance of "robot" class, and the new extended ASV is an instance of "ditch- 
robot" class. The latter class is defined as a subclass of the former class. Thus, the entire functionality of 
the "robot" class became available to the "ditch-robot" class through the inheritance mechanism. The newly 
required capabilities were added to the "ditch-robot" class using "defmethod" which defines the functionality 
of a class in Flavors. The result was remarkable. The additional code written for the new "ditch-robot" 
class was less than 10% of the size of the original "robot" class, and roughly more than 95% of the original 
code was reused. 

One of advantages of rule-based control of motion coordination is the ease of extension of 
coordination logic resulting from the fact that individual rules or a group of rules define an independent 
piece of behavior. Instead of rewriting all the code related to motion coordination, the new ditch crossing 
coordinator was simply added to the original Prolog code. In order to accept the new coordinator, the 
original plan Prolog goal in [16] was subdivided into two plan subgoals y free _gaitsjnotion -Coordi- 
nation _plan and ditch_cros sing jnotionjcoor dination _plan . The old plan code in [16] was merely renamed 
as free __g aits _motion_coor dination _plan without any further modification, and the new ditch grossing _mo- 
tionjcoor dination _plan code was simply added. If the original motion coordinator logic had been imbedded 
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in the "ASV" robot code because only one programming paradigm, such as an imperative paradigm, had 
been utilized to program the work in [15], the extension to a ditch crossing capability in the "ASV" robot 
code would have been a very difficult and very time consuming task. 

Overall, the development and coding of the new extended "ASV" and motion coordinator clearly 
manifested the advantages of the use of multiple programming paradigms to program a complex robot 
motion coordination function which constantly performs on-line optimization like a human or an animal 
coordinating his or its motion based on sensory information and learned experiences. Rule-based 
programming to express logic, object-oriented programming to simulate physical and functional objects, 
and a numerical processing library written in a functional or imperative language to implement mathematics 
and physics needed for simulation are very naturally divided components to simulate a complex system, 
such as that treated in this report. 

One of major complains about programs using Artificial Intelligence techniques and languages is 
slow execution speed in on-line computing applications. In this study reported here, the most prominently 
visible candidate to be blamed for slow execution speed is Prolog code. However, the execution speed of 
Symbolics Prolog on a Symbolics Lisp machine is not so slow as might be expected. It is only slightly 
slower than that of Symbolics Lisp or Flavors. However, the execution speed of Prolog implementations 
on other machines are usually considerably slower than those of non-Prolog implementations. One 
solution for slow Prolog execution speed may be to use a special Prolog processor, such a Xenologic X-l 
[27], to execute Prolog code. The other solution is to convert the Prolog code to an other language, such as 
Lisp. The latter approach was actually adopted to test the correctness of the program developed herein, 
using a TI Explorer machine because this solution is readily applicable without great modification to the 
interface between Prolog and Lisp codes, and because a TI Explorer machine was conveniently available to 
the authors in an office environment. Though the speed gain in program execution is little over that 
expected with Symbolics Prolog, this approach potentially makes a much wider variety of computing 
hardware suitable to execute the motion coordination program developed here. Moreover, this approach may 
provide another advantage in near future since advances in microprocessors based on RISC or CISC 
architecture [28] will, with respect to Lisp execution speed, soon equal or outperform Lisp machines [20]. 
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Already, with respect to execution speed alone, SPARC-based Sun workstations narrow the large gap 
previously existing between a Lisp machine and a conventional machine running the Lisp language. 
Therefore, rather than running a slow Prolog program on a conventional machine, automatic conversion 
from Prolog to Lisp after a development phase could become an effective way to achieve markedly better 
performance if the program were to be tested on the physical AS V walking machine. 



7. Summary and Recommendation 

The main purpose of this study was to demonstrate the value of a multiple pr*-* ramming paradigm 
approach in the development of software for motion coordination for the ASV walking machine. An 
important secondary goal was extending the work in [16] so that the ASV can cross a ditch without any 
assistance from a human operator. Thus, the terrain handling capability of the ASV under program control 
was extended from binary-type terrain to ternary -type terrain for the first time. The third goal was to take 
into consideration the overlapping working volumes of the legs of the ASV in order to utilize the full 
kinematic capability that the vehicle geometry can give. This later factor made a direct contribution in 
widening the maximum ditch width (MDW) that the vehicle can cross. 

The approach adopting multiple programming paradigms for motion coordination, which was 
proposed in [11] and implemented in [16], again exhibited its power. First of all, it forced a well-organized 
and functionally clean abstraction hierarchy for a complex and ill-defined problem. Secondly, it 
considerably reduced development time and effort The program development associated with this report 
could have been a major undertaking if a single programming paradigm had been utilized. Instead, as 
described in the preceding text with the approach taken here most of the program in [16] is reused, while 
only small amount of code is additionally written. 

The code translation from Prolog to Lisp was possible because the Prolog code used herein was 
utilized as a simple rule-based system. This success of this translation further justifies the usage of Prolog 
as one of the languages in the multiple paradigm environment because it could allow much wider varieties 
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of computing hardware to execute the motion coordination program developed. Moreover, this actually 
made the program execution somewhat faster than that of the program with the untranslated Prolog code. 

Among studies remaining to be conducted are inclusion of vehicle inertia in the simulation, effects 
of leg motion on the location of the vehicle center of gravity, and a better simulation of the vision system. 
Such a study would be appropriate to a later phase of this research along with an investigation of further 
changes to the Prolog rule set to enable the ASV to climb over large obstacles or to go around them if this 
is not possible. 
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Figure 1: Typical Simulation Terrain and Vehicle 





Figure 2: Hierarchy of simulation objects 
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Cycle 1 :State1 : Place Leas in the Air 



3 




Cvclel :State2: Back Middle Leas 







Cvclel :State6: Lift Middle Leas and Move 



Figure 3: Ditch Crossing Preparation Phase 
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Q ytie2:S tatel: B ac k Mi ddle Legs 





Cvcle2:State3: Forward Middle Leas 



Figure 3: Continued ... 
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u« stability 




Cvcle3:State1 : Move Forward Front Leas 



Move Rear Le 9 s 




Cvcle3:State2: Move Back Middle Leas 



'f'yiTii 



Move Middle Le 9 s 





Cvcle3:State3: Move Forward Rear Leas 
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Cvcle4:State1 : Move Forward Front Leas 



Figure 4: Ditch Crossing Main Phase 
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Stability 




Cvcle5:State1 : Move Forward Front Leas 





0y fle5:$tate2: Mqv? Bac k Mi ck l e Legs 






Q ydaS:Stata3: Mqvq Forward Rear Leg? 



Figure 4: Continued ... 
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Mode.PROLOGPackage: robot-rulesBase:10 



robot initialize, repeat, myjoop, fail, 
initialize inits, init_ditch_plan. 

init_dicth_plan retract(plan_cycle(_))> retract(plan_state(J)> fail. 

init_ditch_plan asserta(plan_cycle(l)), 

asserta(plan_state(place_legs_in_the_air)). 

myjoop get_command, plan, execute, !. 

get_command X is read joystick. 



plan ditch_mode, ditch_crossingjnotion_coordination_plan. 
plan free_gaits_motion_coordination_plan. 

ditch_mode ditch_mode(in). ;cleared by ditch_plan. 
ditch_mode X is at_ditch_area, X = t, asserta(ditch_mode(in)). 



execute execute_motion, draw_robot, !. 
execute jnotion X is execute_planned_motion. 
draw_robot X is graphical_display. 



Figure 5: Prolog Program 
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..^c ****** ***************************************** ********** 

;; Free Gaits Motion Coordination Plan 
..********************************************************** 



free_gaits_motion_coordination_plan :- update_robot_state, checkjjanjirnit, 

leg_plan, body_plan, generate_decision, !. 

u pda te_robot_s tate X is updaie_robot_status. 



check_tkm Jimit A_leg is at_tkm _limit, A_leg V= nil, 
asserta(Umit_leg(A_leg,lift)). 

check_tkjn_lirnit. 

leg_plan lift_a_leg. 
leg_plan exchange_legs. 
leg_plan stable. 
leg_plan place_a_leg. 
leg_plan wait_for_legs. 

stable Condition is stable_p, Condition — L 

lift_a_leg stable, A_leg is smallest_tkm_leg, A_leg V== nil, 

Condition is stable_without(A_leg), Condition == t, 
asserta(decision(AJeg,_,lift)). 

exchangejegs stable, LegA is smallest_tkm Jeg, LegA \= nil, 
LegB is max_sm Jeg(Leg A) , LegB \= nil. 
Condition is has_more_tkm(LegB ,LegA), 
Condition == t, 

asserta(decision(LegAJLegB,exchange)). 

place_a_leg AJeg is max_sm_legO> A_leg \== nil, 
asserta(decision(A_leg,_,place)). 

wait_for_legs try_new_foothold. 
wait_for_legs recovery, asserta(reduce_speed). 
wait_for_legs asserta(reduce_speed), restore_limit_leg. 

try_new_foothold AJeg is leg_with_new_foothold, AJeg\== nil, 
asserta(decision(A_leg w ,place)). 

recovery A_leg is do_recovery, A_leg \== nil, 

asserta(decision(AJeg, _, place)), restore_limitJeg. 

restore_limit_leg retract(limitJeg(AJeg,lift)). 
restore_limitJeg. 



Figure 5: Continued ... 
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********************************************************** 



Ditch Crossing Motion Coordination Plan 
********************************************************** 



ditch_crossing_motion_coordination_plan ditch_plan_done, retractfditclwnodefin)), idle_cycle. 
ditch_crossing_motion_coordination j)lan cycle_planner. 



********** 



Cycle Planner ************ 



ditch j>lan_done plan_cycle(6) t retract(plan_cycle(6)), 
asserta(plan_cycle{ 1 )), 
prepare_next_di tch_plan . 

prepare_next_ditch_plan move. 

cycle_planner one_cycle_done, plan_cycle(N), N1 is N+l, 

retract(plan_cycle(N)) t asserta(plan_cycle(Nl)), 
idle_cycle. 
cycle_planner plan_cycle. 



********** plan Cycle dispatcher ************ 



one_cycle_done plan_state(one_plan_cycle_done), 

retract(plan_state(one_plan_cycle_done)), 

initialize_plan_state. 



initial ize_plan_state as serta(plan_state (start)). 



plan_cycle plan_cycle(l), update_robot_state, ditch_plan_cycle_l, 
body_plan, generate_decision,!. 

plan_cycle plan_cycle(2), update_robot_state, ditch_plan_cycle_2, 
body_plan, generate_decision,!. 

plan_cycle plan_cycle(J), update_robot_state, ditch_plan_cycle_3, 
body_plan, generate_decision,L 

plan_cycle plan_cycle(4), update_robot_state, ditch_j)lan_cycle_4, 
body_plan, generate_decision,L 

plan_cycle plan_cycle(5), update_r obot_s tate , ditch_plan_cycle_5, 
body_plan, generate_decision,!. 

idle_cycle update_robot_state, body_plan, generate_decision, !. 

Figure 5: Continued ... 
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********** 



Cycles 



********* 



ditch_plan_cycle_l :- plan_state(start), retract(plan_state(start)X 
asserta(plan_state(place_legs_in_the_air)), 
place_legs_in_the_air(back_middlejegs). 
ditch_plan_cycle_l place_legs_in_the_air(back_rniddle_legs). 
ditch_plan_cycle_l back_middleJegs(forward_rear_legs). 
ditch _plan_cycle_l forward_rear_legs(fc^ward_rniddle_legs). 
ditch_plan_cycle_l forward_middle_legs(fonvard_frontJegs). 
ditch_plan_cycle_l forward_frontJegs(lift_middle_legs_and_move). 
ditch_plan_cycle_l lift_middle_lcgs_and_move(onc_plan_cycle_done). 



ditch_plan_cycle_2 plan_state(start), retract(plan_state(start)X 
asserta(plan_state(back_middle_legs)), 
back_middle_legs(forward_rear_legs). 
ditch_j>lan_cycle_2 back_middle_legs(forward_rear_legs). 
ditch_j>lan_cycle_2 forward_rear_legs(forward_middle_legs). 
di tc h_plan_cy cle_2 forward_middle_legs(one_j>lan_cycle_done). 



ditch_plan_cycle_3 plan_state(start), retract(plan_state(start)), 

asserta(plan_state(move_forward_frontJegs)), 
move_forward_front_legs(move_forward_middle_legs). 
ditch_j>lan_cycle_3 move_forward_front_legs(move_back_middleJegs). 
ditch _j>lan_cycle_3 move_back_middle_legs(move_forward_rcarJegs). 
ditch_plan_cyclc_3 movc_forward_rcar_lcgs(onc_plan_cyclc_donc). 



ditch_plan_cycle_4 plan_state(start), retract(plan_state(start)X 

asserta(plan_state(move_forward_middle_legs)), 
move_forward_middlejegs(onc_plan_cycle_done). 
ditch_plan_cycle_4 move_forward_middle_legs(one_j)lan_cycle_done). 



ditch_j>lan_cycle_5 plan_state(startX retract(plan_state(start)X 

asserta(plan_statc(move_forward_front_legs)), 
move_forward_front_legs(move_forward_middle_legs). 
ditch_j>lan_cycle_5 move_forward_front_legs(move_back_middJe_legs). 
ditch_plan_cycle_5 move_back_middle_legs(move_forward_rcar_legs). 
ditch_plan_cycle_5 move_forward_rear_legs(one_j>lan_cycle_done). 



Figure 5: Continued ... 
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..************** Slates *************** 



;;;;; back_middlejegs subgroup 

back_middle_legs(Next_State) plan_state(back_middlejegs), 

back_middleJegs_done, 

retract(plan_state(back_middle_legs)), 

asserta(plan_state(Next_State)), 

stop. 

back_middleJegs(Next_State) plan_state(back_middlejegs), 

do_back_m iddle Jegs , 
stop. 



;;;;; forward_front_legs subgroup 

forward_front_legs(Next_State) plan_state(forward_front_legs)), 

forward_firont_legs_done, 

retract(plan_state(forward_front_legs), 

asserta(plan_state(Next_State)), 

stop. 

forward_front_legs(Next_State) plan_state(forward_front_legs)), 

do_forward_£ront_legs, 

stop. 



;;;;; forward_middle_legs subgroup 

forwaid_middle_legs(Next_State) plan_state(forward_rniddleJegs), 

forward_middle_legs_done, 

retract(plan_state(forward_middleJegs), 

asserta(plan_state(Next_State)), 

stop. 

forward_middleJegs(Next_State) plan_state(forward_middJeJegs), 

doJbrward_middleJegs, 

stop. 



;;;;; forward_rear_legs subgroup 

forward_rear_legs (Nex t_S tate) plan_state(forward_rearJegs), 

f orward_rear _legs_done , 
retract(plan_state(forward_rearJegs), 
asserta(pl an_state(N ex t_S tate)) , 
stop. 

forward_rear_legs(Next_State) plan_stite(forward_rear_legs), 

doJorward_rearJegs, 

stop. 



;;;;; lifMniddle_legs_and_move subgroup 

lift_middle_legs_and_move(Next_State) plan_state(lift_middle_legs_and_move), 

move_done, stop, 

Figure 5: Continued ... 
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retract(plan_state(lift_middleJegs_and_move)), 

asserta(plan_state(Next_State)). 

Iift_middle_legs_and_move(Next_State) plan_stale(lift_middle_legs_and_move), 

do_iift_middle Jegs, move. 



;;;;; move_back_middle_legs subgroup 

move_back_middle_legs(Next_State) plan_state(move_back_middle_legs), 

move_back_middle_legs_done, 
retract(plan_state(move_back_middle_legs)), 
asserta (pi an_state(N ex t_S tate)) . 

move_back_middle_legs(Next_State) plan_state(move_back_middle_legs), 

do_move_back_middle_legs. 



;;;;; move_forward_front_legs subgroup 

move_forward_front_legs{Next_State) plan_state(move_forward_front_legs), 

move_forward_front_legs_done, 
retract(plan_state(move_forward_frontJegs)), 
asserta(plan_state(Nex t_State)) . 

move_forward_front_legs{NexuState) plan_state(move_forward JrontJegs), 

do_move_forward_fron tjegs. 



;;;;; move_forward_middle_legs subgroup 

move_forward_rnidd]e_legs(Next_State) plan_state(move_forward_middle_legs), 

move_forward_middle_legs_done, 
retract(plan_state(move_forward_front_legs)), 
asserta (plan_state(Next_S tate)) . 

move_forward_middle_legs(Next_State) plan_state(move_forward_middle_legs), 

do_move_forward_middle_legs. 



;;;;; moveJforward_rear_legs subgroup 

move_forward_rear_legs(Next_State) plan_state(move_forward_rear_legs), 

moveJforward_middle_legs_done, 
retract(plan_state(move_forward_rear_legs)), 
asserta(plan_state(Nex t_State)) . 

move_forward_rear_legs(Next_State) plan_state(move_forward_rear_legs), 

do_m o ve_forw ard_rear_legs . 



;;;;; place_legs_in_the_air subgroup 

place_legs_in_the_air(Next_State) plan_state(place_legsjn_the_air), 

place_legs_in_the_air_done, 

retract(plan_state(placejegs_in_the_air)), 

asserta(plan_state(Next_state)), 

stop. 

place_legs_in_the_air(Next_S tate) plan_state(place_legs_in_the_air) , 

do_place_legs_in_the_air, stop. 



Figure 5: Continued ... 
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**************** Slate Executors ******************** 



;;;;; do_back_middle_legs subgroup 

back_m iddle_legs_done all_middle_legs_lifted, all_middle_legs_placed, 

clear_middle_Iifted_memory , clear_move_memory . 
do_back_middle_legs all_middlejegsjifted, place_middle_legs_back. 
do_back_middle_legs lift_middlejegs. 

all_middle_legs_lifted midd]e_legs(lifted). 
all_middle_legs_lifted X is both_middleJegs_lifted, X = t, 
asserta(middle_legs(Hfted)). 

all_middle_legs_placed X is both_middle_legs_placed, X = t. 

clear_middlc_lifted_memory retract(middlejegs0ifted)). 

place_middle_legs_back AJeg is placable_middle_leg, AJegW nil, 

asserta(decision(A_leg,_,place_back)). 

place_middle_legs_back. 

lift_middle_legs AJeg is liftable_middle_leg, AJegW nil, 
asserta(decision(AJeg,_,lift)). 

lift_middlejegs. 



;;;;; dojorwardjrontjegs subgroup 

forward_frontJegs_done alljrontjegs_lifted, all_firontJegs_placed, 

clear JrontJifted_memory, clear_move_memory. 

do_fonvard_firontJegs allJrontJegs_lifted, place_frontJegs. 
do_forward_firont_legs lift_frontJegs. 

all_frontJegs_lifted frontjegs(lifted). 
all_frontJegsJifted X is both_frontJegsJifted, X == t, 
asserta(front_legs(lifted)). 

all_frontJegs_placed X is both_frontJegs_placed, X = L 

clearJrontJifted_memory retract(frontJegs(lifted)). 

place_£ront_legs AJeg is placable_firont_leg, AJeg \== nil, 
asserta(decision(AJeg^, place)). 

place_frontJegs. 

lift_frontJegs AJeg is liftable_frontJeg, AJeg\== nil, 
asserta(decision(AJeg,_,lift)). 

lift_frontJegs. 



Figure 5: Continued ... 
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;;;;; do_forward_middle_legs subgroup 

forward_middleJegs_done all_middle_legs_lifted, all_middle_legs_placed, 

clear_middle_lifted_memory, clear_move_memory. 

do_forward_middle_legs ail_middle_legs_lifted, place_middle_legs. 
doJorward_middle_legs lift_middle_legs. 

place_middle_legs A_leg is placable_middle_leg, A_leg\= nil, 
asserta(decision(A_leg,_,place)). 

place_middle_legs. 



;;;;; do_forward_rear_legs subgroup 

fonvard_rear_legs_done all_rear_legs_lifted, all_rcar_legs_placed, 

clear_rear_lified_memory, clear_move_memory. 

do_forward_rearJegs ail_front_legs_lified, place_rear_legs. 
do_forward_rcar_legs lifl_rcar_legs. 

all_rear_legs_lifted rearjegs(lifted). 
all_rear_legs_lified X is both_rear_legs_l if ted , X = t, 
asserta(rear_legs(lifted)). 

all_rear_legs_placed X is both_rear_legs__placed, X = t. 

clear_rcar_lifled_memory re tract(rear_legs (lifted)). 

place_rear_legs A_leg is placable_rear_leg, AJeg\== nil, 
asserta(decision(A_leg,_, place)). 

place_rear_legs. 

lift_rear_legs A_leg is liftable_rear_leg, AJeg\== nil, 
asserta(decision(A_leg,_,lift)). 

lift_rear_legs. 



;;;;; do_lift_middle_legs subgroup 
do_lift_middle_legs lift_middle_legs. 



;;;;; do_move_back_middle_legs subgroup 

move_back_middle_legs_done all_middle_legs_lifted, all_middle_legs_placed, 

clear_middle_lifted_memory, clear_move_memory, 
stop. 

do_move_b^ck_middle_legs all_middle_legs_lifted, m overdone, stop, 

place_m iddle_legs_back . 

do_move_back_middle_legs all_middle_legs_lifted, move. 
do_move_back_middle_legs lift_middlejegs, stop. 



Figure 5: Continued ... 
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;;;;; do_move_forward_front_legs subgroup 

move_forward_front_legs_done all_front_legs_Jifted, all_front_legs_placed, 

clear_front_lifted_memory, clear_move_memory, 
stop. 

do_move_forward_frontJegs all JrontJegsJifted, move_done, stop, 

place_front_legs. 

do_move_forward_front_legs aIl_frontJegs_lifted, move. 
do_move_forward_front_legs lift_front_legs, stop. 



;;;;; do_move_fonvard_middle_legs subgroup 

move_fonvard_middle_legs_done all_middlejegs_lifted, 

all_middlejegs_placed, 
clear_middle_lifted_memory, 
clear_move_memory, stop. 

do_move_forward_middleJegs all_middle_legsjifted, move_done, stop, 

place_middlejegs. 

do_move_forward_middle_legs all_middle_legs_Iifted, move. 

do_move_fonvard_middle_legs lift_middlejegs, stop. 



;;;;; do_move_fonvard_rear_legs subgroup 

move_for\vard_rear_legs_done all_rearjegs_lifted, all_rear_leg s_placed , 

clear_rearJifted_memory, clear_move_memory, 
stop. 

do_move_forward_rear_legs all_rear_legs_lifted, move_done, stop, 

place_rear_legs. 

do_move_forward_rear_legs all_rearjegs_lifted, move. 
do_move_forward_rear_legs lift__rearjegs, stop. 



;;;;; placejegs_in_the_air subgroup 

place_legs_in_the_air_done X is all_legs_placed, X = L 

place Jegs_in_the_air AJeg is placablejeg, A_leg W nil, 
as serta(decision(AJeg,_, place)). 

place_legs_in_the_air. 



;;;;; body_movement subgroup 

move asserta(resume_movement). 
stop asserta(stop_movement). 

clear_move_memory retract(move(done)). 
clear_move_memory. 

move_done move(done). 

move_done X is at_tkm_limit, X\== nil, asserta(move(done)). 
move_done X is at_stability_limit, X \= nil, asserta(move(done)). 

Figure 5: Continued ... 



41 



..********************************************************** 

** 

»» 

;; Plan Libraries 

»* 

..***********♦**%**********************♦******************** 

»* 

;;;;; body_plan subgroup 

body_plan speed_plan, trajectory_plan. 

speed_plan retract(reduce_speed) , slow_down. 
speed_plan speed_up. 

speed_up X is speed_up_robot. 

slow_down X is slow_down_robot. 

trajectory _plan stable_m, restore_trajectory. 
trajectory _plan modify_trajectory. 

stable_m Condition is stable_p_m, Condition == t. 

restore_trajectory X is restore_command. 

modify_trajectory X is modify_command. 



;;;;; generate_decision subgroup 

generate_decision retract(decision(A Jeg3 Jeg,A_decision)), 

X is send_decision(A_leg,BJeg A_decision), fail. 
generate_decision retrac t(l im it_leg( A_leg , A_dec is ion )) , 

X is send_decision(A_leg,_,A_decision), fail. 

generate_decision. 



Figure 5: Continued ... 
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Appendix 



Lisp Code for ASV Simulation 



body-controller-t . lisp 



Wed Nov 28 10:11:54 1990 



1 



;;; Mode : Common-Lisp; Base: 10 -*- 

* ***************************************************************** 
/ 

; body-controller definition 
/ 

.***★***********★★************************★********************★** 



(defflavor body-controller {joystick-command-regulator terrain-regulator 

H-calculator 

body-trans-ratel body-rot ate-ratel 
body-trans-rate6 body- rot ate -rate 6 
body-trans-ratel 0 body-rotate- rate 10 
HI inv-Hl H6 inv-H6 H10 inv-HIO 
H inv-H body-t body-r) 



0 

: initable-instance-variables) 



(defmethod (body-controller :initti) 

0 

(setf joystick-cornmand-regulator (make-instance ' joystick-command-regulator) ) 
(setf terrain-regulator (make-instance 'terrain-regulator)) 

(setf H-calculator (make-instance 'H-calculator)) 

(send joystick-command-regulator :initti) 

(send terrain-regulator :initti) 

(setf H (send H-calculator rinitti)) 

(send self : init-body-rates) 

(send self :init-H) 

HI 

) 



(defmethod (body-controller : init-body-rates ) 

0 

(setf body-trans-ratel '(0 0 0)) 

(setf body-trans-rate6 ' (0 0 0)) 

(setf body-trans-ratelO '(0 0 0)) 

(setf body-rotate-ratel ' (0 0 0) ) 

(setf body-rotate-rate6 ' (0 0 0) ) 

(setf body-rotate-ratelO ' (0 0 0) ) 

) 



(defmethod (body-controller :init-H) 

0 

; library fucntion : ident 
(setf HI H) 

(setf H6 H) 

(setf H10 H) f 

(setf inv-H (matrixinv H) ) 

(setf inv-Hl inv-H) 

(setf inv-H6 inv-H) 

(setf inv-HIO inv-H)) 



body-controller-t . lisp 



Wed Nov 28 10:11:54 1990 



2 



(defmethod (body-controller rcontrol) 

(joystick-command deceleration-f actor estimated-support-plane) 

(setf H HI) 

(send self :update joystick-command deceleration-factor estimated-support-plane) 
(send self :save) 

(dotimes (i 10) 

(cond ( (equal i 0 ) 

(setf body-trans-ratel body-t) 

(setf body-rotate-ratel body-r) 

(setf HI H) 

(setf inv-Hl inv-H) ) 

( (equal i 5) 

(setf body-trans-rate6 body-t) 

(setf body-rotate-rate6 body-r) 

(setf H6 H) 

(setf inv-H6 inv-H) ) 

( (equal i 9 ) 

(setf body-trans-ratelO body-t) 

(setf body-rotate-ratelO body-r) 

(setf H10 H) 

(setf inv-HIO inv-H) ) ) 

(send self : update joystick-command deceleration-factor estimated-support-plane) 

) 

(send self rrestore) ) 



(defmethod (body-controller rupdate) 

(joystick-command deceleration-factor estimated-support-plane) 
; internally used by control method 

(let* ( (t-command (send terrain-regulator : regulate 

estimated-support-plane H) ) 

(j-command (send joystick-command-regulator : regulate 

joystick-command deceleration-factor) ) 

) 

(setf body-t (list (first j-command) (second j-command) 

(third t-command) ) ) 

(setf body-r (list (first t-command) (second t-command) 

(third j-command) ) ) 

(setf H (send H-calculator :new-H body-t body-r)) 

(setf inv-H (matrixinv H) ) ) ) 



(defmethod (body-controller : restore) 

0 

; internally used by control method 

(send joystick-command-regulator rrestore) 
(send terrain-regulator rrestore) 

(send H-calculator rrestore)) 



(defmethod (body-controller rsave) 

<-) 

; internally used by control method 

(send joystick-command-regulator rsave) 
(send terrain-regulator rsave) 

(send H-calculator rsave)) 



body-controller-t . lisp 



Wed Nov 28 10:11:54 1990 



3 



(defmethod (body-controller : get-body-trans-ratel ) 

0 

body-trans-ratel) 



(defmethod (body-controller : get-body-rotate-ratel) 

0 

body-rot ate- r ate 1 ) 



(defmethod (body-controller : get-body-trans-ratelO ) 

0 

body-trans-ratelO) 



(defmethod (body-controller :get-body-rotate-ratelO) 

0 

body- rot ate- rat el 0 ) 



(defmethod (body-controller :get-Hl) 



0 



HI) 



(defmethod (body-controller :get-inv-Hl) 

() 

inv-Hl) 



(defmethod (body-controller :get-H6) 

0 

H6) 



(defmethod (body-controller :get-inv-H6) 

0 

inv-H6) 



(defmethod (body-controller :get-H10) 



0 



H10) 



(defmethod (body-controller :get-inv-H10) 

0 



inv-HIO) 
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... Mode : Common-Lisp; Base: 10 -*- 



body flavor definition 



(def flavor body (stability-calculator support-plane-estimator 

body-controller owner 
estimated-support -plane 
deceleration- factor 
support-plane-clock 
modify- vector 
modify-vector-p 
st op-mot ion- flag 
joy-command) 

0 

: initable-instance-variables ) 



(defmethod (body : slow-down) 

0 

(setf deceleration-factor < + deceleration-factor 1) ) 
(if (> deceleration-factor 20) 

(setf deceleration-factor 20) ) ) 

(defmethod (body ; speed-up) 

0 

(setf deceleration-factor (- deceleration-factor 1)) 
(if (< deceleration-factor 0) 

(setf deceleration-factor 0))) 



(defmethod (body :stable-m) 

(supporting- legs) 

(send stability-calculator :stable-m 

supporting-legs (send body-controller :get-H10))) 



(defmethod (body :stable-p-m) 

(supporting-p-legs a-leg) 

(send stability-calculator :stable-p-m 

supporting-p-legs 

(send body-controller :get-Hl))) 



(defmethod (body :stop-p) 

0 

(let ( (trans-rate (send self : get-body-trans-ratel) ) ) 
(equal (list (first trans-rate) 

(second trans-rate) ) 

' ( 0.0 0 . 0 ) )) ) 



(defmethod (body :modif y-command) 

0 

(setf modify-vector 
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(send stability-calculator : get-recovery-vector ) ) ) 



(defmethod (body :modify-command-p) 

0 

(setf modify-vector-p 

(send stability-calculator : get-recovery-vector-p) ) ) 



(defmethod (body : restore-cornmand) 

0 

(setf modif y-vector ' (0 0 0))) 



(defmethod (body : restore-cornmand-p) 

0 

(setf modify-vector-p ' (0 0 0))) 



(defmethod (body : stop-mot ion) 
(a-leg) 

(setf stop-motion-flag a-leg)) 



(defmethod (body : restore-mot ion) 

0 

(setf stop-motion-flag nil) ) 



(defmethod (body :initti) 

0 

(setf deceleration-factor 0) 

(setf modify-vector-p '(0 0 0)) 

(setf modify-vector ' (0 0 0)) 

(setf stop-motion-flag nil) 

(setf support-plane-clock 10) 

(setf stability-calculator 

(make-instance 'stability-calculator) ) 

(setf support -plane-estimator 

(make-instance ' support -plane-estimator ) ) 
(setf body-controller 

(make-instance 'body-controller) ) 

(send stability-calculator :initti) 

(send support -plane-estimator rinitti) 

(send body-controller :initti) 

) 

(defmethod (body : get -modif y-vector ) 

0 

(vectsub modify-vector 

(dotprod modify-vector 

(normalize-vector joy-command) ) ) ) 



(defmethod (body : get-modif y-vector-p) 

0 

modify-vector-p) 
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(defmethod (body : calculate-motion) 

(joystick-command legs) 

(setf joy-command joystick-command) 

(cond ( (equal support-plane-clock 10) 

; ??? bug ??? 

(setf estimated-support-plane 

(send support-plane-estimator :get-plane legs) ) 

(setf support-plane-clock 0) ) ) 

(setf support-plane-clock (+ support-plane-clock 1) ) 

(cond 

( (or stop-motion-flag (null modif y-vector-p) ) 

(send body-controller : control 

' (0 0 0 ) 

0 estimated-support-plane) ) 

(modif y-vector-p 
(send body-controller : control 

(vectadd joy-command (send self : get-modif y-vector-p) ) 
deceleration-factor estimated-support-plane) ) 

(t 



(control body-controller 

(vectadd joy-command (send self : get-modif y-vector) ) 
deceleration-factor estimated-support-plane) ) ) ) 



(defmethod (body :get-estimated-support-plane) 

0 

estimated-support -plane) 



(defmethod (body : get-body-trans-ratel ) 

0 

(send body-controller : get-body-trans-ratel ) ) 



(defmethod (body : get-body-rotate-ratel) 

0 

(send body-controller : get-body-rotate-ratel ) ) 



(defmethod (body :get-body-trans-ratelO) 

0 

(send body-controller :get-body-trans-ratelO) ) 



(defmethod (body :get-body-rotate-ratelO) 

0 

(send body-controller :get-body-rotate-ratelO) ) 



(defmethod (body :get-Hl) 

0 

(send body-controller :get-Hl) ) 



(defmethod (body :get-inv-Hl) 
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0 

(send body-controller :get-inv-Hl) ) 



(defmethod (body :get-H6) 

0 

(send body-controller :get-H6)) 



(defmethod (body :get-inv-H6) 

0 

(send body-controller :get-inv-H6) ) 



(defmethod (body :get-H10) 

0 

(send body-controller :get-H10) ) 



(defmethod (body : get-inv-HIO ) 

0 

(send body-controller : get-inv-HIO ) ) 



(defmethod (body rmore-stable) 

(supporting-legs legl leg2) 

(send stability-calculator :more-stable 

supporting-legs (send body-controller :get-H10) 
legl leg2) ) 



(defmethod (body : stable) 

( supporting-legs ) 

(send stability-calculator ratable 

supporting-legs (send body-controller :get-H10))) 



(defmethod (body :stable-p) 

(support ing-p-legs ) 

(send stability-calculator :stable-p 

supporting-p-legs (send body-controller :get-Hl) ) ) 



t 
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;;; Mode : Common-Lisp; Base: 10 



♦ ★Hr******************************************************** 
/ 

; regulator flavor definition 

.★★★★★★**************************************************** 

/ 



(def flavor regulator ( (max-a 3.2174) (time-const 0.5) (sampling-time 0.1)) 

0 

; init able -instance -variables) 



(defmethod (regulator tfilter) 

(desired-x present-x) 

; first order regulation 

(let ( (del-vel (/ (- desired-x present-x) time-const))) 

( + (* (send self :g-limitor del-vel) sampling-t ime) 
present-x) ) ) 



(defmethod (regulator :g-limitor) 

(del-vel) 

; limit acceleration to 3.2174 ft/ (sec*sec) or 0.1 G. 
(cond ( (> del-vel max-a) max-a) 

( (< del-vel (- max-a) ) (- max-a) ) 

(T del-vel) ) ) 



/ 

; joystick-command-regulator flavor definition 
.***************★★**★★★*★*********★★**★*******★**★★*★***★** 



(def flavor joystick-command-regulator (body-trans-rate-x 

body-trans-rate-y 
body-rot ate -rat e-z 
old-body-trans-rate-x 
old-body-trans-rate-y 
old-body-rotate-rat e-z) 



(regulator) 

: initable-instance-variables) 



(defmethod (joystick-command-regulator :initti) 

0 1 

(setf body--trans-rate-x 0.0) 

(setf body-trans-rate-y 0.0) 

(setf body-rotate-rate-z 0.0) 

(list body-trans-rate-x body-trans-rate-y body-rotate-rate-z)) 
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(defmethod (joystick-command-regulator : regulate) 

( joys tic k-command decele rat ion-f actor) 

(if (<- deceleration-factor 0) 

(setf deceleration-factor 0.5)) ; remove effect of deceleration-factor, 

(let* ( (d-const 0.5) 

(x (* (first joystick-command) (/ d-const deceleration-factor) ) ) 

(y (* (second joystick-command) (/ d-const deceleration-factor))) 

(r (* (third joystick-command) (/ d-const deceleration-factor)))) 

(setf body-t rans-rate-x (send self : filter x body-trans-rate-x) ) 

(setf body-trans-rate-y (send self : filter y body-trans-rate-y) ) 

(setf body-rotate-rate-z (send self : filter r body-rotate-rate-z) ) ) 

(if (< (abs body-trans-rate-x) 0.02) (setf body-trans-rate-x 0.0)) 

(if (< (abs body-trans-rate-y) 0.02) (setf body-trans-rate-y 0.0)) 

(if (< (abs body-rotate-rate-z) 0.005) (setf body-rotate-rate-z 0.0)) 
(list body-trans-rate-x body-trans-rate-y body-rotate-rate-z) ) 



(defmethod (joystick-command-regulator : restore) 

0 

(setf body-trans-rate-x old-body-t rans-rate-x) 

(setf body-trans-rate-y old-body-t rans-rate-y) 

(setf body-rotate-rate-z old-body-rotate-rate-z) 

(list body-trans-rate-x body-trans-rate-y body-rotate-rate-z) ) 



(defmethod ( joystick-command-regulator :save) 

0 

(setf old-body-trans-rate-x body-trans-rate-x) 

(setf old-body-trans-rate-y body-trans-rate-y) 

(setf old-body-rotate-rate-z body-rotate-rate-z) 

(list body-trans-rate-x body-trans-rate-y body-rotate-rate-z) ) 
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;;; -*- Mode : Common-Lisp; Base: 10 -*- 

. ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★it 

/ state flavor definition 

.★★★★★★★★★★★★A************************************************* 

(def flavor state (name next-state) 

0 

: initable-instance-variables) 



(defmethod (state 

0 



name) 



: state-name) 



(defmethod (state : set-next-state) 
(a-state) 

(setf next-state a-state) ) 



★ ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★a-* 



sync-state flavor definition 



(def flavor sync-state ( (time 0) (del-t 0.1) time-out) 
(state) 

: initable-instance-variables ) 



(defmethod (sync-state : change) 

0 

(setf time ( + time del-t) ) 
(cond ( (>= time time-out) 
(setf time 0) 
next-state) 

(t self) ) ) 



(defmethod (sync-state :get-time) 

0 

time) 

< 



. ********************************************************** 
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; async-state flavor definition 
. ********************************************************** 



(def flavor async-state (( command nil) (observation nil)) 
(state) 

: initable-instance-variables) 



(defmethod (async-state : change) 

(given-command observed-event) 

(cond ( (and (not observation) 

(equal given-command command) ) 
next-state) 

( (and (not command) 

(equal observed-event observation) ) 
next-state) 

(t self))) 



; state-machine flavor definition 

•★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★A** 



(def flavor state-machine (state owner) 

0 

: initable-instance-variables) 



(defmethod (state-machine : state-name) 

0 

(send state : state-name) ) 



• ★★★★★★★★★★★★★★★★★★★★★★★it******************************* 

; control-state-machine flavor definition 
/ 

•★★★★★★★★★★★★★★★★★★★★★★★★★★a:**************************** 



(def flavor control-state-machine ( (command nil) (observation nil) 

contact-sensor executor) 



(state-machine) 

: initable-instance-variables) 
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(defmethod (control-state-machine :initti) 

(leg-name ) 

(if (member leg-name ' (legl leg4 leg5) ) 

(send self : init-cont rol-machine 'support) 
(send self : init-control-machine 'ready)) 

(setf contact-sensor (send owner : contact-sensor ) ) 
(setf executor (send owner :executor) ) ) 



(defmethod (control-state-machine : init-control-machine) 
(a-state-name) 

; internally used by init method 

(let (return lift support contact descent advance ready) 
(setf return 

(make-instance ' sync-state 

:name 'return : time-out 0.6)) 



(setf lift 

(make -instance 



(setf support 

(make -instance 



(setf contact 

(make- instance 



(setf descent 

(make -instance 



(setf advance 

(make -instance 



(setf ready 

(make -instance 



' sync-state 

:name 'lift : time-out 0.4 
:next-state return) ) 

' async-state 

: name 'support .‘command 'recover-command 
:next-state lift)) 

' sync-state 

:name 'contact : time-out 1.0 
:next-state support) ) 

' async-state 

:name 'descent : observation 'contact-confirm 
:next-state contact) ) 

' sync-state 

:name 'advance : time-out 0.6 
:next-state descent)) 

' async-state 

:name 'ready : command 'deploy-command 
:next-state advance) ) 



(send return : set-next-state ready) 



state (cond ( (equal a-state-name 
ready) 


(send 


ready : 


state-name) ) 


( (equal a-state-name 
advance) 


(send 


advance 


: state-name) ) 


( (equal a-state-name 
descent) 


(send 


descent 


:state-name) ) 


( (equal a-state-name 
contact) 


(send 


contact 


: state-name) ) 


( (equal a-state-name 
support) 


(send 


support 


: state-name) ) 


( (equal a-state-name 
lift) 


(send 


lift : state-name) ) 


( (equal a-state-name 
return) ) ) 


(send 


return 


: state-name) ) 



) 
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; (defmethod (cont rol-state-machine :change :before) 
() 

; (cond ( (typep state ' async-state) 

; (if (contact-sensor : sensing) 

; (setf observation 'contact-confirm) 

; (setf observation nil) ) 

; ) ) ) 



(defmethod (control-state-machine : change) 

0 

(cond ((typep state 'async-state) 

(if (send contact-sensor : sensing) 

(setf observation 'contact-confirm) 

(setf observation nil) ) 

) ) 

(cond ((typep state 'sync-state) 

(setf state (send state rchange))) 

(t (setf state (send state rchange command observation)))) 

; ) 



; (defmethod (control-state-machine rchange rafter) 

; 0 

; send command to executor with sync-state-time 
(send executor : send-command (send state : state-name) ) 
(if (typep state 'sync-state) 

(send executor : set -time (send state : get -time) ) 
(send executor : set-time nil))) 



(defmethod (control-state-machine : send-command) 
(a-command) 

(setf command a-command) ) 
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;;; -*- Mode : Common- Lisp ; Base: 10 -*- 






; display . globals 

.★★★★★★★★★★★★★★★★★★★★★★★★★★★★A***************************** 



(defvar eye-space nil) 

(defvar middle-of-screen nil) 



(defvar terrain- joystick) 
(defvar graph-terrain) 
(defvar graph-asv) 






; display . library 

•★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★A 



(defun draw-to-earth (a-point) 

(let ( (draw-pt (make-displayable 
middle-of-screen 
(transform eye-space a-point) ) ) ) 
(draw-to 

(list (truncate (first draw-pt) ) 
(truncate (second draw-pt) ) ) 
*robot-window*) ) ) 



(defun draw-to-earth-d (a-point) 

(let ( (draw-pt (make-displayable 
middle-of-screen 

(transform eye-space a-point) ) ) ) 
(draw-to-d 

(list (truncate (first draw-pt)) 
(truncate (second draw-pt) ) ) 

* robot-window* ) ) ) 



(defun erase-to-e arth (a-point) 

(let ( (draw-pt (make-displayable 
middle-of-screen 
(transform eye-space a-point) ) ) ) 
(erase-to 

(list (truncate (first draw-pt) ) 
(truncate (second draw-pt))) 
*robot-window*) ) ) 



(defun eye-trans (eye-pt) 

; eye-pt (radius alpha beta) 
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; eye := orient*trans (0,0, -r) *rot (x, -beta) *rot (y, -alpha) *trans (-x, -y, -z) 

; returns eye-space 

; library : ident, transmat , rotate, matrixmult 
(let* ( (orient (ident) ) 

(rot nil) (trans nil) (eye nil) 

(radius (first eye-pt) ) (alpha (second eye-pt) ) (beta (third eye-pt) ) 
(center-of-interest (list (/ (send graph-terrain :max-x) 2) 

(/ (send graph-terrain :max-y) 2) 0))) 

(setf (aref orient 2 2) -1.0) 

(setf trans (transmat 0 0 (- radius) ) ) 

(setf eye (matrixmult orient trans) ) 

(setf rot (rotatemat 'y-axis (- alpha))) 

(setf eye (matrixmult eye rot) ) 

(setf rot (rotatemat 'x-axis (- beta))) 

(setf eye (matrixmult eye rot) ) 

(setf trans (transmat (- (first center-of-interest) ) 

(- (second center-of-interest) ) 

(- (third center-of-interest) ) ) ) 

(matrixmult eye trans) ) ) 



(defun make-displayable (middle pt) 

(let ((scale 5000.0) 

(x (first pt) ) (y (second pt) ) (z (third pt) ) ) 
(list ( + (* scale (/ x z) ) (first middle)) 

(+ (* scale (/ y z) ) (second middle))))) 



(defun move-to-earth (a-point) 

(let ( (draw-pt (make-displayable 
mi ddle-of -screen 

(transform eye-space a-point) ) ) ) 
(move-to 

(list (truncate (first draw-pt)) 
(truncate (second draw-pt) ) ) ) ) ) 






joystick flavor definition 



(defflavor joystick (( joy-x 0) (joy-y 0) (joy-r 0) ) 

0 

: ini table -ins t ance - va r iable s ) 



(def method (joystick : get- joy- value) 

0 

(let* ( (key-value) 

(delta-x 0.2) (delta-y 0.1) (delta-r 0.01)) 

(setf key-value (my-read-char-no-hang) ) 

(cond ((equal key-value ' #\f ) (setf joy-x (+ joy-x delta-x))) 
((equal key-value ' #\b) (setf joy-x (- joy-x delta-x))) 
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(cond 

(cond 

(cond 

(cond 

(list 



((equal key-value '#\r) 
((equal key-value '#\1) 
((equal key-value ' #\=) 
((equal key-value ' #\-) 



(setf joy-y 
(setf joy-y 
(setf joy-r 
(setf joy-r 



( (>= joy-x 2) (setf joy-x 2)) 

( (<= joy-x -2) (setf joy-x -2))) 

( {>- joy-y 1) (setf joy-y 1)) 

( (<= joy-y -1) (setf joy-y -1))) 

( {>- joy-r 0.1) (setf joy-r 0.1)) 

( (<= joy-r -0.1) (setf joy-r -0.1))) 
((equal key-value ' #\q) (setf joy-x 
(setf joy-y 0) (setf joy-r 0))) 
joy-x joy-y joy-r (equal key-value ' 



(- joy-y delta-y ) ) ) 

( + joy-y delta-y) ) ) 

(- joy-r delta-r) ) ) 

( + joy-r delta-r) ) ) ) 



0 ) 

#\x) ) ) ) 



(defmethod (joystick : reset) 

0 

(setf joy-x 0) 

(setf joy-y 0) 

(setf joy-r 0) ) 



(setf terrain- joystick (make-instance ' joystick) ) 



9 

; terrain flavor definition 

.******************************************************** 

/ 



(defflavor terrain ( (terrain-data (make-array '(49 49) : initial-element 0)) 

terrain-height-array terrain-height-list joystick 
(cursor-x) (cursor-y) (max-x) (max-y) 

(radius 500) (alpha 0) (beta 0) ) 

0 

: initable-instance-variables 
: get table -instance -variables ) 



(defmethod (terrain : create) 

0 

(send self rinitti) 

(send self rmodify) 

(my-print "Now use joystick to control the robot.")) 



(defmethod (terrain :kill) 

<> 

(kill-robot -ter rain- windows) 
(restore-lisp-listener) ) 



(defmethod (terrain :initti) 

0 

; globals : middle-of-screen, eye-space 
(move -and-shape-lisp- listener) 
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(let ( (array-dims (array-dimensions terrain-data) ) ) 

(setf radius 500 alpha 0 beta 0) 

( set f max-x (first array-dims) ) 

(setf max-y (second array-dims)) 

(setf cursor-x (floor (/ max-x 2))) 

(setf cursor-y (floor (/ max-y 2)))) 

(setf terrain-height-array (make-array (+ max-x 1) ) ) 

(make -robot -window) 

(setf middle-of-screen 

(middle-of-robot-window) ) 

(setf eye-space (eye-trans (list 500 0 0) ) ) 

(send self : input-terrain-parameters ) 

(my-print "Please use joystick to transform the terrain.”) 
(my-print "Wait.”) 

(make-visible) 

(send self : erase-obstacles ) 

(my-print "Now you can translate the terrain.”)) 



(defmethod (terrain :modify) 

0 

; external : eye-space 
(do ( (delta 0.0001) 

(joystick-value nil) 

(end-flag nil) ) 

(end-flag (my-print "Wait.”) 

(send joystick :reset) 

(send self : save-terrain eye-space) 

(send self .-draw-obstacles) ) 

(make-visible) 

(setf joystick-value (send joystick :get- joy-value) ) 

(let ( (x (first joystick-value) ) 

(y (second joystick-value) ) 

(r (third joystick-value) ) 

(fire (fourth joystick-value) ) ) 

(send self rerase-terrain) 

(cond 

(fire (cond ( (user-ok) 

(cond ( (user-save) 

(send self : save-terrain-to-disk (user-file-name)))) 
(setf end-flag t) ) 

(t 

(send joystick :reset) 

(setf joystick-value (send joystick : get- joy-value) ) ) ) ) 
(setf alpha (+ alpha 0.05))) 

(setf 
(setf 
(setf 
(setf 



( (> 
( « 
( (> 
( « 
( (> 
( « 
(setf 
(send 



delta) 

(- delta) ) 
delta) 

(- delta) ) 
delta) 

(- delta) ) 



alpha (- alpha 0.05))) 
beta ( + beta 0.05))) 
beta (- beta 0 . 05) ) ) 
radius (+ radius 10))) 



(setf radius (- radius 10)))) 



eye-space (eye-trans (list radius alpha beta) ) ) 
self : draw-terrainl eye-space) ) ) ) 



(defmethod (terrain : in-side-of-whole-terrain) 

(a-pos) 

(let ( (dimension-terrain (array-dimensions terrain-data) ) 
(i-x (floor (first a-pos) ) ) 

(i-y (floor (second a-pos)))) 

(cond ( (< i-x 0) nil) 
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( (< i-y 0) nil) 

( (> i-x (- (first dimension-terrain) 1) ) nil) 
( (> i-y (- (second dimension-terrain) 1) ) nil) 
(T) ) 

)) 



(defmethod (terrain :permitted-cell) 

(terrain-pos ) 

(let ( (i-x (floor (first terrain-pos) ) ) ; find terrain index 

(i-y (floor (second terrain-pos)))) 

(if (send self : in-side-of-whole-terrain terrain-pos) 

(if (equal (aref terrain-data i-x i-y) 0) ; permitted 

t 

nil) ) ) ) 



(defmethod (terrain : terrain-point ) 

(a-pos-wrt -earth) 

(let* ( (x (first a-pos-wrt -earth) ) 

(y (second a-pos-wrt-earth) ) 

(z (send self :get-height (list x y) ) ) ) 
(list x y z) ) ) 



(defmethod (terrain :get-height) 

(a-pos-wrt-earth) 

; range 0 =< x <« (first dimension-terrain-height ) , (0 < x <39) 

; 0 =< y <= (second dimension-terrain) . 

(let* ( (dimension-terrain-height (array-dimensions terrain-height-array) ) 
(x-min 0) (x-max (first dimension-terrain-height)) 

(x (first a-pos-wrt-earth) ) ) 

(if (or (< x x-min) (> x x-max) ) 

-1000 

(let* ( (i-x (floor x) ) ; get terrain x-index 

(xl (if (< (- x i-x) 0.5) (- i-x 1) i-x)) 

(x2 (if (< (- x i-x) 0.5) i-x ( + i-x 1))) 

(xl (if (< xl x-min) 0 xl) ) 

(x2 (if (>= x2 x-max) (- x-max 1) x2) ) 

(zl (aref terrain-height -array xl) ) 

(z2 (aref terrain-height-array x2) ) 

(slope (- z2 zl) ) 

(del-x (- x xl) ) ) 

(+ zl (* slope del-x) ) ) ) ) ) 



(setf graph-terrain (make-instance 'terrain 

: joystick terrain- joystick) ) 



< 



;★*★★★★★*★*★*★★★*★★*★★**★*★★★★********★*★★*★*******★★★★★★★* 



; terrain . input -terrain-parameters 
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.★★*★*•★★★★★★★*■★*★★★★★★•*★*★★★★*★★*★★★*★★★★★★*■*★**★■**■***■*■**** 



(defmethod (terrain : input-terrain-parameters) 

0 

(initialize -menu -variables ) 

(cond ( (setf *old-terrain-f ile-name* (get-old-terrain-file-name) ) 

(send graph-terrain : read-terrain-f rom-disk *old-terrain-f ile-name* ) ) 
(t 

(send self : get-new-terrain) ) ) ) 



(defmethod (terrain : get-new-terrain) 

0 

(send self : get-new-terrain-height ) 
(send self :draw-terrain eye-space) 
(send self : set-new-terrain-obstacles ) 
(send self : set-new-ditch) ) 



(defmethod (terrain : get-new-terrain-height ) 

0 

(let ( (slope-type (get-terrain-slope-type) ) 

(angle nil) (data nil) ) 

(cond ((equal slope-type 'single-angle) 

(setf angle (get-terrain-slope-angle) ) ) 

((equal slope-type 'manual) 

(setf data (get-terrain-slope-data) ) ) ) 

(my-print "Wait.”) 

(send self : read-terrain-height slope-type angle data))) 



(defmethod (terrain : set-new-terrain-obstacles) 

0 

(let ( (terrain-type (get-terrain-obstacle-type) ) 

(values nil) 

(obstacle-ratio nil) (random-seed nil) ) 

(cond ((equal terrain-type 'random) 

(setf values (get-terrain-random-data) ) 

(setf obstacle-ratio (first values)) 

(setf random-seed (second values) ) 

(my-print "Wait.") 

(send self : random-terrain obstacle-ratio random-seed)) 
(t 

(send self : display-cursor) ) ) ) ) 



(defmethod (terrain : set-new-ditch) 

0 

(let ( (ditch-type (get-ditch-type) ) 

(width-location nil) ) 

(cond ((equal ditch-type 'add-ditch) 

(setf width-location (get-ditch-width-location) ) 
(my-print " Wa it . " ) 

(send self : add-ditch (first width-location) 
(second width-location) ) 

(send self : draw-obstacles ) 

(make-visible) ) 
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(t nil) ) ) ) 



.a********************************************************* 

/ 



; terrain .display-terrain 

. a********************************************************* 
r 



(defmethod (terrain :display-cursor) 

0 

(send self :make-all-permitted) 

(do ((joy-data nil) (x nil) (y nil) (r nil) (fire nil) 

(exit-flag nil) ) 

(exit-flag (send self : erase-cursor (list cursor-x cursor-y) ) ) 
(make -visible ) 

(setf joy-data (send joystick : get- joy-value) ) 

(setf x (- (second joy-data))) (setf y (first joy-data)) 

(setf r (third joy-data)) (setf fire (fourth joy-data)) 

(send self rerase-cursor (list cursor-x cursor-y) ) 

(cond 

(fire (setf exit-flag t) ) 



( (> 


X 


0) 


(setf 


cursor-x 


(+ cursor-x 


1) ) 


(if 


(> cursor-x max-x) 

(setf cursor-x max-x) ) ) 


( « 


X 


0) 


(setf 


cursor-x 


(- cursor-x 


1) ) 


(if 


(< cursor-x 0) 

(setf cursor-x 0))) 


( (> 


y 


0) 


(setf 


cursor-y 


(+ cursor-y 


1) ) 


(if 


(> cursor-y max-y) 

(setf cursor-y max-y) ) ) 


( « 


y 


0) 


(setf 


cursor-y 


(- cursor-y 


1) ) 


(if 


(< cursor-y 0) 

(setf cursor-y 0))) 


( « 


r 


0) 


(setf 


(aref terrain-data cursor 


-x 


cursor-y) 1) ) 


( (> 


r 


0) 


(setf 


(aref terrain-data cursor 


-x 


cursor-y) 1) ) ) 



(send self :draw-cursor (list cursor-x cursor-y) ) 
(send self :draw-obstacles) 

(send joystick rreset))) 



(defmethod (terrain : draw-terrain) 

(eye-space) 

; external function: \display . library\move-to-earth, draw-to-earth 
(dotimes (x ( + max-x 1) ) 

(move-to-earth (list x 0 (aref terrain-height-array x) ) ) 
(draw-to-earth (list x max-x (aref terrain-height-array x) ) ) ) 
(dotimes (y (+ max-y 1) ) 

(move-to-earth (list 0 y 0) ) 

(dotimes (x (+ max-x 1)) 

(draw-to-earth (list x y (aref terrain-height-array x) ) ) ) ) ) 



(defmethod (terrain :draw-terrainl) 

-(eye-space) 

; external function: \ display . library\move-to-earth, draw-to-earth 
(do ( (xs (list 0 max-x) (cdr xs) ) 

(x nil) ) 

( (null xs) ) 

(setf x (car xs) ) 

(move-to-earth (list x 0 (aref terrain-height-array x) ) ) 
(draw-to-earth (list x max-x (aref terrain-height-array x) ) ) ) 
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(do ( (ys (list 0 max-y) (cdr ys) ) 

(y nil) ) 

( (null ys) ) 

(setf y (car ys) ) 

(move-to-earth (list 0 y 0) ) 

(dotimes (x ( + max-x 1) ) 

(draw-to-earth (list x y (aref terrain-height-array x) ) ) ) ) ) 



(defmethod (terrain : erase-obstacles) 

0 

; externals : terrain 

; external function: \display . library\move-to-earth, draw-to-earth 
(dotimes (i (first (array-dimensions terrain-data) ) ) 

(dotimes (j (second (array-dimensions terrain-data))) 

(cond ( (equal 1 (aref terrain-data i j) ) 

(move-to-earth (list i j)) 

(erase-to-earth (list (+ i 1) (+ j 1) ) ) 

(move-to-earth (list (+ i 1) j)) 

(erase-to-earth (list i (+ j 1)))))))) 



(defmethod (terrain : erase-terrain) 

0 

(clear-robot-window) ) 



(defmethod (terrain :make-all-permitted) 

0 

(dotimes (i max-x) 

(dotimes (j max-y) 

(setf (aref terrain-data i j) 0) ) ) ) 



(defmethod (terrain : read-terrain-height ) 

(terrain-slope-type terrain-slope-angle terrain-slope-data) 
(cond ((equal terrain-slope-type 'default) 

(setf terrain-height-list '((19 0) (25 1) (35 1.5)))) 

((equal terrain-slope-type 'single-angle) 

(let* ((angle (* pi (/ terrain-slope-angle 180))) 

(max (* 20 (tan angle)))) 

(setf terrain-height-list 
(list ' (20 0) 

(list 40 max) 

) ) ) ) 

(t (setf terrain-height-list terrain-slope-data) ) ) 

(let* ( (xl 0) (zl 0) (a-pair) (zz 0) 

(x2 (first (car terrain-height-list) ) ) 

(z2 (second (car terrain-height-list))) 

(slope (/ (- z2 zl) (- x2 xl) ) ) ) 

(setf terrain-height-list (cdr terrain-height-list) ) 

(dotimes (i (+ max-x< 1) ) 

(setf zz (+ (* slope (- i xl) ) zl) ) 

(cond ( (equal x2 i) 

(setf xl x2) 

(cond ( (setf a-pair (car terrain-height-list) ) 

(setf terrain-height-list (cdr terrain-height-list)) 
(setf x2 (first a-pair) ) 

(setf z2 (second a-pair) ) 

(setf zl zz) 
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(setf slope (/ (- z2 zl) (- x2 xl) ) ) ) 

(T (setf slope 0) (setf zl zz) ) ) ) ) 
(setf (aref terrain-height-array i) zz) ) ) ) 



(defmethod (terrain : save-terrain) 
(eye-space) 

(send self : draw-obstacles ) 

(send self : draw-terrain eye-space) 
( save -terra in-to- terrain-buffer) ) 



(defmethod (terrain : save-terrain-to-disk) 

(file-name) 

(with-open-f ile 
(out-file 

(merge-pathnames file-name "robot : kwak . robot . terrain-data 

) 

(setf *print -array* t) 

(print terrain-data out-file) 

(print terrain-height-array out-file) 

(print radius out-file) 

(print alpha out-file) 

(print beta out-file) 

(setf *print-array* nil))) 



(defmethod (terrain : read-terrain-f rom-disk) 

( file-name) 

(with-open-f ile 
(input-file 

(merge-pathnames file-name "robot : kwak . robot . terrain-data ; 
(setf *print-array* t) 

(setf terrain-data (read input-file)) 

(setf terrain-height-array (read input-file) ) 

(setf radius (read input-file) ) 

(setf alpha (read input-file) ) 

(setf beta (read input-file) ) 

(setf *print-array* nil))) 



★★★A****************************************************** 



terrain .display-cursor 



********************************************************** 



(defmethod (terrain : draw-cursor) 
( position ) 

(let* ( (x (first position)) 



(y .(second position)) 








(pi (list 


(+ X 0.2) 


( + 


y 


0.2) 


0) ) 


(p2 (list 


(+ x 0.8) 


( + 


y 


0.2) 


0)) 


(p3 (list 


(+ x 0.8) 


< + 


y 


0.8) 


0) ) 


(p4 (list 


(+ x 0.2) 


( + 


y 


0.8) 


0) ) 


(points (list p2 p3 


p4 


pD ) ) 





(move-to-earth pi) 

(do ( (points points (cdr points) ) ) 



data") :direction :outp 



") rdirection : input) 



display-t2 . lisp 



Wed Nov 28 10:10:22 1990 



10 



((null points) 'done-draw-cursor) 
(draw-to-earth (car points))))) 



(defmethod (terrain : draw-obstacles ) 

() 

(dotimes (i max-x) 

(dotimes (j max-y) 

(cond ( (equal 1 (aref terrain-data i j) ) 

(move-to-earth 

(list i j (aref terrain-height-array i) ) ) 

(draw-to-earth 

(list ( + i 1) (+ j 1) (aref terrain-height-array ( + i 1)))) 

(move-to-earth 

(list (+ i 1) j (aref terrain-height-array ( + i 1)))) 
(draw-to-earth 

(list i (+ j 1) (aref terrain-height-array i) )))))) ) 



(defmethod (terrain : erase-cursor ) 
( position ) 

(let* ( (x (first position) ) 

(y (second position) ) 



(pl 


(list 


( + 


X 


0.2) 


( + 


y 


0.2) 


0) ) 


(p2 


(list 


< + 


X 


0.8) 


( + 


y 


0.2) 


0) ) 


(p3 


(list 


< + 


X 


0.8) 


( + 


y 


0.8) 


0) ) 


(p4 


(list 


< + 


X 


0.2) 


< + 


y 


0.8) 


0) ) 



(points (list p2 p3 p4 pi) ) ) 
(move-to-earth pi) 

(do ((points points (cdr points))) 

((null points) 'done-erase-cursor) 
(erase-to-earth (car points))))) 



(defmethod (terrain : random-terrain) 

(obstacle-ratio random-seed) 

(let ( (a 43411) (b 17) (c 640001) (percent nil) (seed nil) (x nil) ) 
(setf percent obstacle-ratio) 

(setf seed random-seed) 

(setf x seed) 

(dotimes (i max-x) 

(dotimes (j max-y) 

(if (< (/ (setf x (mod ( + (* a x) b) c) ) c) (/ percent 100)) 
(setf (aref terrain-data i j) 1))))) 

(send self : draw-obstacles) ) 



(defmethod (terrain : add-ditch) 

(width location) 

(dotimes (i width) 

(dotimes (j max-y) f 

(setf .(aref terrain-data (+ i location) j) 1) ) ) ) 



.********************************************************** 

/ 

; graph-vehicle flavor definition 
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.********x************************************************* 



(defflavor graph-vehicle ( (vehicle-points (make-array 28)) 

(body-points (make-array 10)) 
(polygons (make-array 13) ) 
(numpolys nil) 

(vertices (make-array 100) ) ) 

0 

: initable-instance-variables) 



(defmethod (graph-vehicle :init-data) 

0 

(send self : read-vehicle-data) ) ; read data from disk 



(defmethod (graph-vehicle :display) 

(a-H foot-positions) 

( clear- robot -window) 

(send self :body-pento-wrt-earth a-H foot-positions) 
(send self : draw-vehicle vehicle-points) 

(copy- ter rain- to- robot -window) 

(make-visible) ) 



(defmethod (graph-vehicle : read-vehicle-data) 

0 

; global variables : vehicle-points, polygons, numpolys, vertices 
; format of file : num-of-points num-of -polygons 
; ( num a-vehicle-point ) .... 

; ( num-of-vertices vertices-number-of-a-polygon) . . . 

(let* ((vehicle-file (open "exp3 : kwak . robot ; vehicle . data" ) ) 

(numpts (read vehicle-file) ) 

(numvtces 0) (a-polygon nil) ) 

(setf numpolys (read vehicle-file) ) 

(dotimes (i numpts) 

(setf (aref vehicle-points i) (cdr (read vehicle-file)))) 

(dotimes (i 10) 

(setf (aref body-points i) (aref vehicle-points i) ) ) 

(dotimes (i numpolys) 

(setf a-polygon (read vehicle-file) ) 

(setf (aref polygons i) (list numvtces (car a-polygon))) 

(do ( (a-polygon-vertices (cdr a-polygon) (cdr a-polygon-vertices ) ) 
(j 0 (+ j 1) ) ) 

( (null a-polygon-vertices) ) 

(setf (aref vertices (4 numvtces j) ) 

(- (first a-polygon-vertices) 1))) 

(setf numvtces (4 numvtces (car a-polygon)))) 

(close vehicle-file) ) ) 



t 

(setf graph-asv (make-instance 'graph-vehicle)) 



★*******★*★★***★★★*★*★★**★★*★★★★★★★*★***★★★*★★★***★★★★**** 



graph-vehicle . display 
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.******■***************'**■*******■*•******•★*★***★***********★** 



(defmethod (graph-vehicle : body-pento-wrt-earth) 

( a-H foot-positions ) 

; library : transform 

(let ((si 0.6616) (s2 0.945) (s3 3.308) (1 0.8133) (m 1.0467) 

; (hipx-list '(5.1667 5.1667 0.0 0.0 -4.9167 -4.9167)) 
(hipx-list '(6.0 6.0 0.0 0.0 -6.0 -6.0)) 

(hipy-list '(1.62 -1.62 1.62 -1.62 1.62 -1.62)) 

(signl-list '(1-1 1-1 1 -1)) 

(sign2-list ' (1 1 1 1 -1 -1) ) ) 

(send self : transform-body-points a-H body-points) 

(do ((positions f oot-pos itions (cdr positions)) 

(hipx-list hipx-list (cdr hipx-list) ) 

(hipy-list hipy-list (cdr hipy-list) ) 

(signl-list signl-list (cdr signl-list) ) 

(sign2-list sign2-list (cdr sign2-list) ) 

(i 0 (+ i 1) ) ) 

( (null positions) nil) 

(let* ( (foot-pos (car positions)) 

(hipx (car hipx-list) ) (hipy (car hipy-list) ) 

(signl (car signl-list) ) (sign2 (car sign2-list) ) 

(px (- (first foot-pos) hipx) ) 

(py (- (second foot-pos) hipy) ) 

(pz (third foot-pos) ) 

(theta (vehicle-theta py pz m signl)) 

(dm (vehicle-dm px sign2) ) 

(dl (vehicle-dl py pz m 1) ) 

(top-pos nil) (knee-pos nil) ) 

(setf top-pos 

(transform a-H 

(vehicle-top-pos hipx hipy m 1 dl theta signl) ) ) 
(setf knee-pos 

(transform a-H 

(vehicle-knee-pos hipx hipy m 1 si s2 s3 
dl dm theta signl sign2))) 

(setf foot-pos (transform a-H foot-pos) ) 

(setf (aref vehicle-points (+ 10 (* 3 i) ) ) 
top-pos ) 

(setf (aref vehicle-points ( + 11 (* 3 i))) 
knee-pos ) 

(setf (aref vehicle-points ( + 12 (* 3 i) ) ) 

foot-pos) ) ) ) ) 



(defmethod (graph-vehicle : draw-vehicle) 

( vehicle-points ) 

; global variables : polygons , numpolys, vertices 

(dotimes (i numpolys) 

(let ((start (first (aref polygons i) ) ) 

(num-vertices (second (aref polygons i) ) ) ) 
(move-to-earth (aref vehicle-points 
(aref vertices start) ) ) 

(dotimes (j num-vertices) 

(draw-to-earth-d (aref vehicle-points 

(aref vertices (+ start j) ) ) ) ) 

) ) ) 
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• ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★I* 
/ 

; graph-vehicle . display . body-pent o-wrt-earth 
/ 

/ 



(defmethod (graph-vehicle : transform-body-points) 
(a-H body-points) 

; globals : vehicle-points 
; library : transform 
(dotimes (i 10) 

(setf (aref vehicle-points i) 

(transform a-H (aref body-points i) ) ) ) ) 



(defun vehicle-dl (py pz m 1) 

(/ (“ (sqrt ( + (* py py) (* pz pz) (- (* m m) ) ) ) 1) 

4)) 



(defun vehicle-dm (px sign2) 
(* sign 2 (/ px 5) ) ) 



(defun vehicle-knee-pos (hipx hipy m 1 si s2 s3 
dl dm theta signl sign2) 

(let* ((numer ( + (* si si) (- (* s2 s2)) (* dl dl) (* dm dm) ) ) 

(denom (* 2 si (sqrt (+ (* dl dl) (* dm dm))))) 

(beta (acos (/ numer denom))) 

(alpha (- (/ pi 2) (atan dm dl) beta) ) 

(sina (sin alpha)) (cosa (cos alpha)) 

(sint (sin theta) ) (cost (cos theta) ) 

(temp (- (* s3 sina) (- dl 1))) 

(xk (+ (* sign2 s3 cosa) hipx)) 

(yk ( + (* signl (+ (* temp sint) (* m cost))) hipy)) 

(zk (- ( + (* temp cost) (* m sint))))) 

(list xk yk zk) ) ) 



(defun vehicle-theta (py pz m signl) 

(let* ( (anglel (atan (* signl py) (* -1 pz) ) ) 
(angle2 (atan m (sqrt (+ (* py py) 

(* pz pz) 

(- (* m m) ) ) ) ) ) ) 

(- anglel angle2) ) ) 



(defun vehicle-top-pos (hipx hipy m 1 dl theta signl) 
(let* ( (xt hipx) 

(1-dl (- 1 dl)) 

(sina (sin theta) ) 

(cosa (cos theta) ) 

(yt ( + (* signl ( + (* m cosa) (* 1-dl sina))) hipy)) 
(zt (- (* m sina) (* 1-dl cosa)))) 

(list xt yt zt) ) ) 
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;;; -*- Mode : Common-Lisp; Base: 10 

* ******************************************************************** 
/ 



; ditch-robot definition 

.*********************************************************★********* 



(defflavor ditch-robot ( ) 

(test -over lap- robot) 



) 



(defmethod (ditch-robot rinitti) 

0 

(send graph-asv :init-data) 

(setf vision-system (make-instance 'ditch-vision-system 
(send vision-system :initti) 

(setf joystick (make-instance ' joystick) ) 

(send joystick : reset) 

(empty-queue lift-queue) 

(setf lift-flag t) 

(let ((H)) 

(setf body (make-instance 'stop-body rowner self)) 

(setf H (send body rinitti)) 

(setf legs (list 

(make-instance 'test-overlap-leg 
(make-instance 'test-overlap-leg 
(make-instance 'test-overlap-leg 
(make-instance 'test-overlap-leg 
(make-instance 'test-overlap-leg 
(make-instance 'test-overlap-leg :name 
) ) 

(mapcar #' (lambda (a-leg) (send a-leg :initti H) ) legs)) 



: owner self) ) 



: name 


' legl 


: owner 


self) 


: name 


' leg2 


: owner 


self) 


:name 


' leg3 


: owner 


self) 


:name 


' leg4 


: owner 


self) 


:name 


' leg5 


: owner 


self) 


:name 


' leg6 


: owner 


self) 



(defmethod (ditch-robot : at-stability-limit ) 

0 

(not (send self :stable))) 



(defmethod (ditch-robot : stop-motion) 

0 

(send body : stop-body-motion) ) 



(defmethod (ditch-robot : resume-motion) 

0 

(send body : restore-body-mot ion) 
t) 



• ★*★★★★★★*★★★*★******★******★★★★★**★****★**★****★*★**★★ ★ 



(defun at_stability_limit () 

(send asv : at-stability-limit ) ) 
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(defun stop_motion () 

(send asv : stop-mot ion) ) 



(defun resume_jnotion () 

(send asv : resume -mot ion) ) 



c 
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;;; -*- Mode : Common-Lisp; Base: 10 -*- 
******************************************************************** 

; ditch-vision-system definition 
$ 

.******************************************************************* 

0 



(def flavor 
) 



ditch- vis ion- system 
(vision-system) 



0 



(defmethod 

(let ( (x 
(cond 



(ditch- vis ion-system : on -ditch-area ) 
(body-HIO ) 

(aref body-HIO 0 3) ) ) 

(and (>- x (- 21 7) ) 

(<- x (+ 21 *ditch-width* ) ) ) 



t) 

(t nil)))) 



(setf *ditch-width* 6) 



c 
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;;; - * - Mode : Common- Lisp; Base: 10 



/ 

; executor flavor definition 



(defflavor executor 

( leg-pos-wrt-body desired-f oothold-pos-wrt -earth 

time command owner sensor (lift-height 1.4) 
(T1 0.6) (T2 1.0) (T3 0.4) (T4 0.6) 

(planned-contact-time 0.4) self-time 
(sampling-time 0.1) ready-pos 

HI inv-Hl body-trans-ratel body-rotate-ratel ) 

0 

: initable-instance-variables) 



(defmethod (executor : set-desired-pos ) 

(a-pos ) 

(setf desired-f oothold-pos-wrt-earth a-pos) ) 



(defmethod (executor : get-desired-pos ) 

0 

desired-f oothold-pos-wrt-earth) 



(defmethod (executor : send-command) 
(a-command) 

(setf command a-command) ) 



(defmethod (executor : set-time) 
(a-time) 

(setf time a-time) ) 



(defmethod (executor : leg-pos-wrt-body ) 

0 

leg-pos-wrt-body) 



(defmethod (executor :move) 

(H inv-H body-trans-rate body-rotate-rate) 
(setf HI H) 

(setf inv-Hl inv-H) 1 

(setf bodyrtrans-ratel body-trans-rate) 

(setf body-rotate-ratel body-rotate-rate) 

(cond ((equal command 'ready) 

(send self :move-in-ready ) ) 

((equal command 'advance) 

(send self :move-in-advance) ) 

((equal command 'descent) 

(send self :move-in-descent ) ) 
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((equal command 'contact) 

(send self :move-in-contact ) ) 
((equal command 'support) 

(send self :move-in-support ) ) 
((equal command 'lift) 

(send self :move-in-lif t ) ) 
((equal command 'return) 

(send self :move-in-return) ) 

) 

) 



(defmethod (executor :move-in-contact ) 

() 

(let ( (leg-velocity-wrt-body (send self 
(setf leg-pos-wrt-body 

(vectadd (magvect sampling-time 
leg-pos-wrt-body) ) ) ) 



: f ind-velocity-wrt-body ) ) ) 
leg-velocity-wrt-body) 



(defmethod (executor : f ind-velocity-wrt-body ) 

0 

; returns f oot-velocity-wrt-body 

; velocity = - ( body-trans-rate + body-rotate-rate X leg-pos ) 

; globals v : body-trans-ratel, body-rotate-ratel 
; lib : vectsub, vectadd, crossprod 
(vectsub '(000) 

(vectadd body-trans-ratel 

(crossprod body-rotate-ratel leg-pos-wrt-body) ) ) ) 



(defmethod (executor :move-in-advance) 

0 

(let ( (desired-pos (send self :desired-advance-pos-wrt-body) ) 
(dt (- T1 time) ) ) 

(send self :move-del desired-pos leg-pos-wrt-body dt)) 

(setf self-time 0.0)) 



(defmethod (executor :desired-advance-pos-wrt-body) 

0 

; a-pos is desired-stepping-pos-wrt-earth 
; returns desired-pos-wrt-body in deploy state 
; global variable : HI, inv-Hl 

; global function : to-earth-t ransf orm, to-body-transf orm, f ind-terrain-hegiht 
(let* ( (desired-pos-wrt-earth desired-f oothold-pos-wrt -earth) 

(terrain-height (third (send owner : terrain-point desired-pos-wrt-earth) ) ) 
(desired-pos-height-wrt-earth (+ terrain-height lift-height) ) 

(pos-wrt -earth (list (first desired-pos-wrt-earth) 

(second desired-pos-wrt-earth) 
desired-pos-height-wrt-earth) ) ) 

(to-body-transf orm inv-Hl pos-wrt -earth) ) ) 



(defmethod (executor :move-in-descent ) 

0 

; global function : to-body-transf orm 
; global variables : inv-Hl 

(let ( (dt (- planned-contact-time self-time) ) ) 
(if (< dt 0.05) 
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(setf leg-pos-wrt-body (to-body-t ransf orm 

inv-Kl desired-f oothold-pos-wrt-earth) ) 

(send self :move-del 

(to-body-transf orm inv-Hl desired-f oothold-pos-wrt-earth) 
leg-pos-wrt-body dt) ) ) 

; ) 



; (defmethod (executor :move-in-descent : after) 

; 0 

(setf self-time {+ self-time sampling-time) ) ) 



(defmethod (executor :move-del) 

(desired-pos present-pos dt) 

; set new leg-pos depending on the arguments 
; lib : vectadd, magvect 
(if ( < dt 0.05) 

(setf leg-pos-wrt-body desired-pos) 

(let* ( (inv-time-dif f (/ 1 dt) ) 

(del (vectsub desired-pos present-pos) ) 

(velocity (magvect inv-time-dif f del) ) ) 

(setf leg-pos-wrt-body 

(vectadd present-pos (magvect sampling-time velocity) ) ) ) ) ) 



(defmethod (executor :move-in-lif t ) 

0 

(let* ( (dt (- T3 time) ) 

(desired-pos (send self : lif t-pos-desired) ) 

(z (third desired-pos))) 

(send self :move-del desired-pos leg-pos-wrt-body dt) 
(setf ready-pos 

(list (first ready-pos) (second ready-pos) z) ) ) ) 



(defmethod (executor : lif t-pos-desired) 

; returens position-wrt-body which will be at the end of lift state. 

; global f : to-body-transf orm, 

; global v : inv-Hl 

0 

(let* ( (leg-pos-wrt-earth (to-earth-t ransf orm HI leg-pos-wrt-body) ) 

(desired-height (+ lift-height (third (send owner : terrain-point leg-pos-wrt-ea 

h))))) 

(to-body-transf orm inv-Hl (list (first leg-pos-wrt-earth) 

(second leg-pos-wrt-earth) 
desired-height) ) ) ) 



(defmethod (executor :move-in-ready) 
0 ’ 

(setf leg-pos-wrt-body ready-pos) ) 



(defmethod (executor :move-in-return) 

0 

; Modifying leg-pos-z is redundent but it can correct disturbance by itself, 
(let ( (dt (- T4 time) ) 
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(desired-pos ready-pos) ) 

(send self :move-del desired-pos leg-pos-wrt-body dt) ) ) 



(defmethod (executor :move-in-support ) 

0 

; globals : body-trans-ratel , body-rotate-ratel 
; lib : vectadd, magvect 

; In general terrain, leg-pos-z should be updated by real terrain height, 
(let ( (leg-velocity-wrt-body (send self : f ind-velocity-wrt-body) ) ) 

(setf leg-pos-wrt-body 

(vectadd (magvect sampling-time leg-velocity-wrt-body) 
leg-pos-wrt-body) ) ) ) 



(defmethod (executor rinitti) 

(leg-name init-H) 

(setf sensor (send owner : contact-sensor ) ) 

(let ( (x (aref init-H 0 3) ) 

(y (aref init-H 1 3) ) 

(z (aref init-H 23))) 

(cond ((equal leg-name 'legl) 

(setf ready-pos ' ( 5 3 -4) ) 

(setf leg-pos-wrt-body (list 6 3 (- z) ) ) 

(setf desired-f oothold-pos-wrt-earth (list (+ x 6) 
((equal leg-name 'leg2) 

(setf ready-pos 9 ( 5 -3 -4)) 

(setf leg-pos-wrt-body (list 5-3 (- z) ) ) 

(setf desired-f oothold-pos-wrt-earth (list (+ x 5) 
((equal leg-name 'leg3) 

(setf ready-pos MO 3 -4)) 

(setf leg-pos-wrt-body (list 0 3 (- z) ) ) 

(setf desired-foothold-pos-wrt-earth (list (+ x 0) 
((equal leg-name 'leg4) 

(setf ready-pos M 0 -3 -4) ) 

(setf leg-pos-wrt-body (list 0-3 (- z) ) ) 

(setf desired-foothold-pos-wrt-earth (list ( + x 0) 
((equal leg-name 'leg5) 

(setf ready-pos 9 (-5 3 -4) ) 

(setf leg-pos-wrt-body (list -5 3 (- z) ) ) 

(setf desired-foothold-pos-wrt-earth (list (- x 5) 
((equal leg-name 'leg6) 

(setf ready-pos 9 (-5 -3 -4) ) 

(setf leg-pos-wrt-body (list -5 -3 (- z) ) ) 

(setf desired-foothold-pos-wrt-earth (list (- x 5) 



(+ y 3) 0) ) ) 

(- y 3) 0) ) ) 

(+ y 3) 0) ) ) 

<- y 3) 0) ) ) 

(+ y 3) 0) ) ) 

(- y 3) 0) ) ) ) ) 
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... Mode : Common-Lisp; Base: 10 -*- 

• ********★★*★★★*★★★**★★*★★★****★*★**★★*★★★★★★★**★★★**★★**★*★*★★★★★★★ 

; load file (f-load-t-444 . lisp) 

.**★★★•*•*•*★•**★★★★★★★•*★*■*■*■*★★★■*★★★■*★★★■*★*★★■*•★★★★★★■*■*★★★**■*■★**•*■**•*•*■■*■*★★ 



graph-terrain is used in sensor and vision 

Overlapped working volume (1 foot) 

Front and rear are not extended. 

logic change 



(load "robot ; kwak . robot ;math-t" ) 

(load "robot :kwak. robot ; user-interf ace-t2" ) 

( load " robot : kwak . robot ; graph-t 1 " ) 

(load "robot : kwak . robot ;display-t2") 

(load "robot :kwak . robot 1 ; vis ion-t" ) 

(load "robot : kwak . robot5 ; ditch-vision-t " ) 

(load "robot : kwak . robot ; tkm-t" ) 

(load "robot :kwak . robot 4 ; overlap-tkm-t " ) 

( load " robot : kwak . robot 1 ; f oothold-t" ) 

(load "robot : kwak . robot4 ; overlap-f oothold-t" ) 

(load "robot : kwak . robot; sensor-t" ) 

(load "robot : kwak . robotl ; executor-t" ) 

(load "robot : kwak . robot ; control-machine-t" ) 

(load "robot : kwak . robot ; plan-machine-t" ) 

(load "robot :kwak . robotl; leg-t") 

(load "robot : kwak . robot 4; overlap-leg-t" ) 

(load "robot : kwak . robot 5 ; test -overlap-leg-t -4 41 " ) 

(load "robot : kwak . robot; stability-t2" ) 

(load "robot : kwak . robot; support-plane-t" ) 

(load "robot : kwak . robot ; h-calculator-t" ) 

(load "robot :kwak. robot ; command- regulator-t") 

(load "robot :kwak . robot ; terrain-regulator-t" ) 

(load "robot : kwak . robot ;body-controller-t") 

(load "robot : kwak . robotl ; body- t" ) 

(load "robot : kwak . robot 6 ; stop-body-t" ) 

(load "robot : kwak . robotl; robot-tl" ) 

(load "robot : kwak . robot 4 ; overlap-robot-t " ) 

( load " robot : kwak . robot 5 ; test-overlap-robot-t-4 42 " ) 
(load "robot : kwak . robot 6 ; ditch-robot-t" ) 

(load " robot ; kwak ♦ robot 6 ; robot 4 44") 

(load "robot ; kwak . robot ; add-to-system-menu") 

(setf asv (make-instance 'ditch-robot)) 
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;;; Mode : Common-Lisp; Base: 10 -*- 

.★★★★★★★****★*★**★**★★***★★★★★*★*★****★★*★★★★★*★★**★★★**★★*★*★★*★* 

/ 

; foothold-finder definition 

•★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★it* 



(defflavor foothold-finder (sixteen-footholds 

four-lines tkm-calculator 
(no-cell-available-flag nil) 
(TKM-margin 0.4) owner) 

0 

: init able -instance -variables ) 



(defmethod (foothold-finder rinitti) 
(leg-name) 

(cond ((equal leg-name 'legl) 

(setf sixteen-footholds 



( ( 


7.3 


4.3) 


( 


7.3 


3.3) 


( 


7.3 


2.3) 


( 


7.3 


( 


6.3 


4.3) 


( 


6.3 


3.3) 


( 


6.3 


2.3) 


( 


6.3 


( 


5.3 


4.3) 


( 


5.3 


3.3) 


( 


5.3 


2.3) 


( 


5.3 


( 


4.3 


4.3) 


( 


4.3 


3.3) 


( 


4.3 


2.3) 


( 


4.3 



1.3) 

1.3) 

1.3) 

1.3) ) ) 



(setf four-lines 

' ( ( (0 0.3420 

( (0 -0.3420 
( (0 -0.3420 
((0 0.3420 

((equal leg-name 'leg2) 

(setf sixteen-footholds 
' ( ( 7.3 -4.3) 

( 6.3 -4.3) 

( 5.3 -4.3) 

( 4.3 -4.3) 

(setf four-lines 

' ( ( (0 0.3420 -0 . 9397) 

( (0 -0.3420 -0.9397) 
( (0 -0.3420 -0.9397) 
((0 0.3420 -0.9397) 

((equal leg-name 'leg3) 

(setf sixteen-footholds 



-0 . 9397) 
-0.9397) 
-0.9397) 
-0.9397) 



( 7.3 -3, 
( 6.3 -3, 
( 5.3 -3, 
( 4.3 -3, 



( 8.0832 2.7339 0)) 

( 8.0832 2.7339 0) ) 

( 3.4167 2.7339 0) ) 

( 3.4167 2.7339 0) ) ) ) ) 



3) ( 7.3 -2.3) ( 7.3 -1.3) 

3) ( 6.3 -2.3) ( 6.3 -1.3) 

3) ( 5.3 -2.3) ( 5.3 -1.3) 

3) (4.3 -2.3) ( 4.3 -1.3)) ) 

( 8.0832 -2.7339 0) ) 

( 8.0832 -2.7339 0) ) 

( 3.4167 -2.7339 0)) 

( 3.4167 -2.7339 0) ) ) ) ) 



(( 1.5 


4.3) 


( 1.5 


3.3) 


( 1.5 


2.3) 


( 1.5 


( 0.5 


4.3) 


( 0.5 


3.3) 


( 0.5 


2.3) 


( 0.5 


(-0.5 


4.3) 


(-0.5 


3.3) 


(-0.5 


2.3) 


(-0.5 


(-1.5 


4.3) 


(-1.5 


3.3) 


(-1.5 


2.3) 


(-1.5 



1.3) 

1.3) 

1.3) 

1.3) ) ) 



(setf four-lines 

' ( ( (0 0.3420 

( (0 
( (0 

((0 0.3420 

((equal leg-name 'leg4) 
(setf sixteen-footholds 



0.9397) 
0.3420 -0.9397) 
0.3420 -0.9397) 
0.9397) 



( ( 1.5 
( 0.5 
(-0.5 
(-1.5 



-4.3) 

-4.3) 

-4.3) 

-4.3) 



( 1.5 -3, 
( 0.5 -3, 
(-0.5 -3 . 
(-1.5 -3, 



(setf four-lines 

' ( ( (0 0.3420 -0.9397) 

( (0 -0.3420 -0.9397) 
( (0 -0.3420 -0.9397) 
( (0 0.3420 -0.9397) 



( 2.2915 2.7339 0) ) 

( 2.2915 2.7339 0) ) 

(-2.2915 2.7339 0) ) 

(-2.2915 2.7339 0))))) 



3) ( 1.5 -2.3) ( 1.5 -1.3) 

3) ( 0.5 -2.3) ( 0.5 -1.3) 

3) (-0.5 -2.3) (-0.5 -1.3) 

3) (-1.5 -2.3) (-1.5 -1.3) ) ) 

( 2.2915 -2.7339 0) ) 

( 2.2915 -2.7339 0) ) 

(-2.2915 -2.7339 0) ) 

(-2.2915 -2.7339 0) ) ) ) ) 



f oothold-t . lisp 



Thu Nov 29 11:29:51 1990 



2 



((equal leg-name 'leg5) 
(setf sixteen-footholds 



( (-4.0 


4.3) 


(-4.0 


3.3) 


(-4.0 


2.3) 


(-4.0 


1.3) 


(-5.0 


4.3) 


(-5.0 


3.3) 


(-5.0 


2.3) 


(-5.0 


1.3) 


(-6.0 


4.3) 


(-6.0 


3.3) 


(-6.0 


2.3) 


(-6.0 


1.3) 


(-7.0 


4.3) 


(-7.0 


3.3) 


(-7.0 


2.3) 


(-7.0 


1.3) ) ) 



(setf four-lines 

'(((0 0.3420 -0.9397) (-3.3332 

((0 -0.3420 -0.9397) (-3.3332 

((0 -0.3420 -0.9397) (-7.8332 

( (0 0.3420 -0 . 9397) (-7.8332 

((equal leg-name 'leg6) 

(setf sixteen-footholds 

'((-4.0 -4.3) (-4.0 -3.3) (-4.0 

(-5.0 -4.3) (-5.0 -3.3) (-5.0 

(-6.0 -4.3) (-6.0 -3.3) (-6.0 



(-7.0 -4.3) (-7.0 -3.3) (-7.0 



(setf four-lines 



2.7339 0) ) 
2.7339 0) ) 
2.7339 0) ) 
2.7339 0) ) ) ) ) 



-2.3) (-4.0 -1.3) 

-2.3) (-5.0 -1.3) 

-2.3) (-6.0 -1.3) 

-2.3) (-7.0 -1.3) ) ) 



' ( ( (0 0.3420 -0.9397) 

( (0 -0.3420 -0.9397) 
( (0 -0.3420 -0.9397) 
( (0 0.3420 -0.9397) 



(-3.3332 -2.7339 0) ) 
(-3.3332 -2.7339 0) ) 
(-7.8332 -2.7339 0) ) 
(-7.8332 -2.7339 0) ) ) ) ) 



) 

(setf tkm-calculator (send owner : tkm-calculator ) ) 

) 



(defmethod (foothold-finder : find-foothold) 

(H6 inv-H6 body-trans-ratelO body-rotate-ratelO 
estimated-support -plane) 

; returns ( (max-foothold max-t)cm) (foothold-list) (tkm-list) ) 

; all points are wpt body coordinate system. 

(let* ( (estimated-support -plane-wrt -body 

(plane-transform estimated-support -plane H6) ) 

(four-points (send self 

: f our-points-on-support -plane 

four-lines est imated-support-plane-wrt-body) ) 
(possible-footholds (send self 

: get-possible-footholds 
(send self 

: estimate- foot holds 

four-points est imated-support-plane-wrt-body) 
H 6 inv-H6) ) ) 

(send self 

: get-f oothold-with-max-TKM 
possible-footholds H6 

body-trans-ratelO body-rotate-ratelO) ) ) 



.»» ******************************************************« 



; foothold-f inder . find-foothold 
• " ★*★*★★*****★★★*★★★★*★★★**★**★*******************★*★*★★ »• 



i 
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(defmethod (foothold-finder : estimate-footholds) 

( four -point s-wrt -body estimated-support-plane-wrt-body) 

; returns estimate-f ootholds-wrt-body 

(do* ((footholds sixteen-footholds (cdr footholds)) 

(out-footholds nil) 

(a-f oothold nil) ) 

( (null footholds) 

(get-points-on-support-plane out-footholds est imated-support-plane-wrt-body) ) 
(setf a-foothold (car footholds) ) 

(if (in-side-of -polygon a-foothold 

(pick-two-dimensions four-point s-wrt-body) ) 

(setf out-footholds (cons a-foothold out-footholds) ) ) ) ) 



(defmethod (foothold-finder : f our-points-on-support-plane) 

(four-lines est imated-support-plane-wrt-body ) 

; returns four points which are intersected by four-lines on 
; est imated-support-plane-wrt-body 
; math lib: plane-intersection 

(do* ( (lines four-lines (cdr lines) ) 

(points nil) ) 

( (null lines) points) 

(setf points (cons (plane-intersection (car lines) 

est imated-support-plane-wrt-body) 



points) ) ) ) 



(defmethod (foothold-finder : get-f oothold-with-max-TKM) 
(possible-footholds H 

body-trans-rate body-rot ate- rate) 

; returns ((max-foothold max-tkm) (foothold-list) (tkm-list) ) 

; sets no-cell-available-flag 

; real-footholds is really possible footholds 

(do ( (footholds possible-footholds (cdr footholds) ) 

(max-foothold nil) (a-foothold nil) (TKM-list nil) (a-TKM nil) 
(real-footholds nil) (max-TKM -100.0)) 

( (null footholds) 

(setf no-cell-available-flag (< max-TKM TKM-margin) ) 

(if (>= max-TKM TKM-margin) 

(make -output -form 

max-foothold max-TKM real-footholds TKM-list H) 
nil) ) 

(setf a-foothold (car footholds) ) 

(setf a-TKM (send tkm-calculator :find-tkm 

a-foothold body-trans-rate body-rotate-rate) ) 

(if a-TKM 

(progn (setf TKM-list (cons a-TKM TKM-list) ) 

(setf real-footholds (cons a-foothold real-footholds)) 
(if (> a-TKM max-TKM) 

(progn (setf max-TKM a-TKM) 

(setf max-foothold a-foothold) )))))) 



(defmethod (foothold-finder : get-possible-footholds) 
(estimated-footholds H inv-H) 

; returns possible-footholds wrt body 
(to-body-transf orm inv-H 

(send self : find-possible-footholds 

(to-earth-transf orm H estimated-f ootholds ) ) 
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) ) 



.»»**★*★*★★**★★*★★★★★★★★★★★****★★**★**★***★*★***★★★*★*★*★»* 



; foothold-* finder . estimate- foot ho Id 



(defun check-polarity (pointl point2 point3) 

(let* ( (vectl (vectsub point2 pointl)) 

(vect2 (vectsub point3 pointl) ) ) 

(if (not (third vectl) ) 

(progn (setf vectl (reverse (cons 0 (reverse vectl)))) 

(setf vect2 (reverse (cons 0 (reverse vect2) ) ) ) ) ) 
(crossprod vectl vect2) ) ) 



(defun get-points-on-support-plane (points est imated-support-plane-wrt-body) 

; returns intersection points with support plane in z-body direction. 

; math lib: plane-intersection 

(do* ( (points points (cdr points) ) 

(out-points nil) ) 

( (null points) out-points) 

(setf out-points (cons (plane-intersection 

(make-line-to-get -point -on-support -plane 
(car points ) ) 

estimated-support -plane-wrt-body) out-points) ) ) ) 



(defun in-side-of-polygon (a-point polygon-points) 

; polygon-points must be convext -polygon and in order & two dimensional points, 
(do* ((first-points polygon-points (cdr first-points)) 

(second-points (reverse (cons (car first-points) 

(reverse (cdr f irst-points ) ) ) ) 

(cdr second-points) ) 

(signs nil) (first-point nil) (second-point nil) ) 

((null first-points) (same-polarity signs)) 

(setf first-point (car first-points) ) 

(setf second-point (car second-points) ) 

(setf signs (cons (check-polarity first-point second-point a-point) 
signs) ) ) ) 



(defun make-line-to-get-point-on-support-plane (a-point) 

; a-point is two dimensional point. 

; returns a-line ( (z-direction) (a-point -100)) 

(list '(0 0 1) (list (fiirst a-point) (second a-point ) -100) ) ) 



(defun pick-two-dimensions (points) 

(if (listp (first points)) 

(do* ( (points points (cdr points) ) 
(a-point nil) 

(out-points nil) ) 



; more than one point case 
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((null points) out-points) 

(setf a-point (car points) ) 

(setf out-points (cons (list (first a-point) (second a-point) ) 

out-points) ) ) 

(list (first points) (second points)))) ; one point case 



(defun same-polarity (signs) 

(do ((signs (cdr signs) (cdr signs)) 

(first-sign (plusp (third (car signs)))) 

(same T) ) 

( (null signs) same) 

(if (not (equal first-sign (plusp (third (car signs))))) 
(setf same nil) ) ) ) 



."★★TAr*****************************************************" 



; foothold-finder . f ind- foot ho Id. get -foot hold-wit h-MAX-t km 

.«********************************************************»* 

/ 



(defun make-output-form 

(max-foothold max-TKM possible-f ootholds TKM-list H) 
; output-form : ( ( f oothold-with-max-tkm t km) 

; (leg-pro ject ed-permit ted- foot holds) 

; (leg-pro jected-TKM-list) ) 

; output footholds are in earth coordinate. 

; math lib : to-earth-transf orm 

(list (list (to-earth-transf orm H max-foothold) max-TKM) 
(to-earth-transf orm H possible-footholds) 

TKM-list) ) 



. •*★★★★**★***★*****★★**★★★*★***★★★★★★★***★★★★★★*★****★***«* 



; foothold-finder . select -f oot hold .get -possible-f oot ho Id 

-•I******************************************************" 



(defmethod (foothold-finder : find-possible-footholds) 

(estimated-f ootholds -wrt -earth) 

; returns possible-f ootholds-wrt -earth 
; graph-terrain is object. 

(do* ( (footholds estimated-f ootholds-wrt -earth (cdr footholds) ) 
(a-foothold nil) (t-cell nil) (out-footholds nil) ) 

( (null footholds) (unique-f ootholds-only out-footholds) ) 
(setf a-foothold (car footholds) ) 

(setf t-cell (get-center-of-digitized-terrain-cell a-foothold) ) 
; (setf out-footholds 

; (cons (list ( + (first t-cell) 0.5) 
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(+ (second t-cell) 0.5) 

0.0) out-footholds)))) 

(if (send owner :permitted-cell t-cell) 

(setf out-footholds 

(cons (send owner : terrain-point t-cell) 
out-footholds) ) ) ) ) 



(defun get-center-of-digitized-terrain-cell (a-foothold) 
; cell resolution is 1 foot by 1 foot 

(list (+ (floor (first a-foothold)) 0.5) 

(+ (floor (second a-foothold)) 0.5))) 



(defun unique-f ootholds-only (mixed-footholds) 

(do* ( (footholds mixed-footholds (cdr footholds) ) 
(out-footholds nil) 

(a-foothold nil) ) 

((null footholds) • out-footholds) 

(setf a-foothold (car footholds)) 

(if (not (member a-foothold out-footholds :test 'equal)) 

(setf out-footholds (cons a-foothold out-footholds))))) 



! 
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;;; Mode : Common-Lisp; Package :USER; Base: 10 -*- 



**************************************x******************* 

low level graph routines 

***★★★★****★★*★★★★★****★*★****★***★*★**★★*★*★★★*★★★★****** 



(defvar 

(defvar 

(defvar 

(defvar 

(defvar 

(defvar 

(defvar 

(defvar 

(defvar 

(defvar 

;TI 

(defvar 

(defvar 



♦robot-display-window* nil) 
♦robot -display- window-array* 
♦robot-window* nil) 
♦robot-window-array* nil) 
♦robot-window-width* nil) 
♦robot-window-height* nil) 
♦terrain-buffer* nil) 
♦terrain-buffer-array* nil) 
*max-y* nil) 

♦start-point* nil) 

*xs* (make-array 2)) 

*ys* (make-array 2) ) 



nil) 



(defun copy-terrain-to-robot-window () 

(tv: sheet -force-access (* robot-window*) 

(send *robot-window* jbitblt 

tv:alu-ior * robot-window-width* *robot-window-height* 
♦terrain-buffer-array* 2 2 0 0) ) ) 



(defun draw-to (a-point a-window) 

; global variables : *start-point * 

(tv: shee t-f or ce -access (a-window) 

(send a-window ' :draw-line (first *start-point *) 
(- *max-y* (second *start-point *) ) 

(first a-point) 

(- *max-y* (second a-point)) tv:alu-ior)) 
(setq *start -point* a-point) ) 



(defun draw-to-d (a-point a-window) 

; global variables : *start-point * 

(tv: sheet-force-access (a-window) 

(setf (aref *xs* 0) ( + 4 (first *start-point* ) ) ) 

(setf (aref *xs* 1) (+ 4 (first a-point) ) ) 

(setf (aref *ys* 0) (+ 4 (- *max-y* (second *start-point * ) ) ) ) 

(setf (aref *ys* 1) (+ 4 (- *max-y* (second a-point)))) 

(send a-window : draw-wide-curve *xs* *ys* 2) ) 

(setq *start-point* a-point) ) 



(defun erase-to (a-point a-window) 

; global variables : *start -point * 

(tv : sheet -force -access (a-window) 

(send a-window ' :draw-line (first *start-point * ) 
(- *max-y* (second *start-point* ) ) 

(first a-point) 

(- *max-y* (second a-point)) tv:alu-andca) ) 
(setq *start-point* a-point) ) 
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(defun get-keyboard-input ( ) 

; This is not for the graphics, but this function uses Zeta LISP. 

; This is the reason why this function is in Zeta graphic package, 
(send *terminal-io* : tyi-no-hang) ) 



(defun make-robot-window () 

(setq *robot-display-window* ( tv :make-window 

' tv : window 
:blinker-p nil 
rposition ' (0 0) 

: width *screen-width* 

rheight (truncate (* 0.8 *screen-height *) ) 
rborders 2 

: label "robot-display-window" 

:name "robot-display-window" 

: save-bits t 
:expose-p t) ) 

(let* ( (r-w (send *robot-display-window* rwidth) ) 

(r-h (send *robot-display-window* :height) ) 

(r-x nil) (r-y nil) ) 

(multiple-value (r-x r-y) (send *robot-display-window* rposition) ) 

(setq *robot-window* (tv rmake-window 9 

tv : window 

:position (list r-x r-y) 

: width r-w 
:height r-h 
:blinker-p nil 
: borders 2 

: label "robot-window" 

:name "robot-window" 

: save-bits t 
:expose-p nil)) 

(setq *terrain-buf f er* (tv :make-window 

' tv : window 

rposition (list r-x r-y) 
rwidth r-w 
rheight r-h 
rblinker-p nil 
rborders 2 

r label "terrain-buffer" 

.-name "terrain-buffer" 
r save-bits t 
rexpose-p nil) ) 

(setq *max-y* (send *robot-window* r inside-height )) ) 

(setq *robot-display-window-array* (send *robot-display-window* rbit-array) ) 
(setq *robot-window-array* (send *robot-window* rbit-array) ) 

(setq *robot-window-width* (send *robot-window* r inside-width ) ) 

(setq *robot-window-height* (send *robot-window* r inside-height ) ) 

(setq *terrain-buf f er-array* (send *terrain-buf fer* rbit-array) ) ) 



(defun make-visible () 

(send *robot-display-window* rbitblt 

tv:alu-seta *robot-window-width* * robot-window-height * 
*robot-window-array* 2200)) 
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(defun move-to (a-point) 

; global variables : *start-point * 

; This function just changes *start-point * . 
(setq *start-point * a-point) ) 



(defun save-terrain-to-terrain-buf f er () 

(tv : sheet -force-access ( *te rra in-buffer * ) 

(send *terrain-buf fer* :bitblt 

tv:alu-seta *robot-window-width* * robot-window-height* 
*robot-window-array* 2 2 0 0) ) ) 



(defun clear-robot-window () 

(tv: sheet -force-access ( * robot -window*) 
(send *robot-window* : clear-window) ) ) 



(defun middle-of-robot-window () 

(list (/ (send *robot-window* : inside-width) 2) 

(/ (send * robot-window* : inside-height ) 2))) 



(defun kill-robot-terrain-windows () 
(send * robot-display-window* :kill) 
(send *robot-window* :kill) 

(send *terrain-buf f er* :kill)) 
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;; -*- Mode : Common-Lisp; Base: 10 -*- 

★★★★★★a************************************************************ 
H-calculator definition 

★★a**************************************************************** 



(def flavor H-calculator ( (sampling-time 

old-H) 



0 

: init able -instance -variables) 



0 . 1 ) 



H 



(defmethod (H-calculator rinitti) 



0 

library fucntion : 


ident 


(setf 


H (ident) 


) 




(setf 


(a ref H 0 


3) 


6.5) 


(setf 


(aref H 1 


3) 


19.5) 


(setf 


(a ref H 2 


3) 


5.4) 


H) 









(defmethod (H-calculator :new-H) 

(body-trans-rate body-rotate- rate) 

(setf H 

(orthogonalization 

(get-new-H 

H 

(get-del-H 

H 

(get-delta body-trans-rate body-rotate-rate sampling-time) ) ) ) ) ) 



(defmethod (H-calculator :save) 

0 

(setf old-H H) ) 



(defmethod (H-calculator .-restore) 

0 

(setf H old-H) ) 



; H-calculator .new-H 

.•»*★*★****★*★★***★***★***★*★*★★**********★********★★**★★" 

c 



(defun get-delta (body-trans-rate 



(let* ( (del-trans-x 


(* 


( first 


(del-t rans-y 


<* 


(second 


(del-trans-z 


<* 


(third 


(del-rotate-x 


(* 


(first 


(del-rotate-y 


(* 


(second 



body-rotate-rate sampling -time) 
body-trans-rate) sampling-time) ) 
body-trans-rate) sampling-time) ) 
body-trans-rate) sampling-time) ) 
body-rotate-rate) sampling-time) ) 
body-rotate-rate) sampling-time) ) 



h-calculator-t . lisp 



Thu Nov 29 11:30:22 1990 



2 



(del-rotate-z (* (third body-rotate-rate) sampling-time) ) ) 
(list (list del-trans-x del-trans-y del-trans-z) 

(list del-rotate-x del-rotate-y del-rotate-z) ) ) ) 



(defun get-del-H (H delta-trans-rotate) 

; math lib : ident 

(let* ( (H-del (ident)) ; initialze identity matirix 

(delta-trans (first delta-trans-rotate) ) 
(delta-rotate (second delta-trans-rotate) ) ) 



(setf 


(aref 


H-del 


0 


0) 


0) 






(setf 


(aref 


H-del 


1 


0) 


(third 


delta-rotate) ) 




( setf 


(aref 


H-del 


2 


0) 


(- (second delta-rotate) 


) ) 


(setf 


(aref 


H-del 


0 


1) 


(- (third delta-rotate) ) 


) 


(setf 


(aref 


H-del 


1 


1) 


0) 






(setf 


(aref 


H-del 


2 


1) 


(first 


delta-rotate) ) 




(setf 


(aref 


H-del 


0 


2) 


(second 


delta-rotate) ) 




(setf 


(aref 


H-del 


1 


2) 


(- (first delta-rotate) ) 


) 


(setf 


(aref 


H-del 


2 


2) 


0) 






(setf 


(aref 


H-del 


0 


3) 


(first 


delta-trans) ) 




(setf 


(aref 


H-del 


1 


3) 


(second 


delta-trans) ) 




(setf 


(aref 


H-del 


2 


3) 


(third 


delta-trans) ) 




(setf 


(aref 


H-del 


3 


*3) 


0) 






(matrixmult 


H H-del) ) ) 









(defun get-new-H (H del-H) 
(matrixadd H del-H) ) 
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;;; Mode : Common-Lisp ; Base: 10 -*- 

r 

; leg flavor definition 
/ 

.**************************************************************** 



(defflavor leg (name owner plan-machine control-machine 

executor contact-sensor tkm-calculator 
foothold-finder exchanged-leg 
foothold tkm foothold-list t km- list tkm-p 
reserved-foothold reserved-tkm) 

0 

: initable-instance-variables 
: get table -instance -variables) 



(defmethod (leg rinitti) 
(H) 



(setf 

(setf 

(setf 

(setf 

(setf 

(setf 

(setf 

(send 

(send 

(send 

(send 

(send 



contact -sens or 
executor 
control -machine 
plan-machine 
tkm-calculator 
foot ho Id- finder 



(make -instance 
(make -instance 
(make -instance 
(make-inst ance 
(make -instance 
(make- instance 



foothold (send executor :initti 
contact-sensor rinitti name) 
control-machine rinitti name) 
plan-machine rinitti name) 
tkm-calculator rinitti name) 
foothold-finder rinitti name) ) 



'contact-sensor rowner self)) 
'executor rowner self)) 

'control-state-machine : owner 
'plan-state-machine rowner 
'tkm-calculator rowner self)) 
'foothold-finder rowner self)) 
name H) ) 



self) ) 
self) ) 



(defmethod (leg : contact-confirm) 

0 

(send contact-sensor rcontact-p) ) 



(defmethod (leg r do-planned-motion) 

0 

(send plan-machine r change) 

(send control-machine r change) 

(send executor rmove (send owner rget-Hl) (send owner rget-inv-Hl) 
(send owner r get-body-trans-ratel) 

(send owner : get-body-rotate-ratel) ) 

(send contact-sensor r sensing)) 



(defmethod (leg rget-Hl) 

0 

(send owner rget-Hl) ) 



(defmethod (leg r has-f oothold-p) 

0 



foothold) 
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(defmethod (leg : interlock-confirm) 

0 

; may add stable-without-p self 

(if (send exchanged-leg : contact-confirm) 
t 

nil) ) 



(defmethod (leg : leg-pos-wrt-body) 

0 

(send executor : leg-pos-wrt-body) ) 



(defmethod (leg : lift-able) 

() 

(if (equal (send plan-machine : state-name) ' eligible-to-lif t ) 
self 
nil) ) 



(defmethod (leg : lift-ok) 

0 

(send owner : lift-ok name)) 



(defmethod (leg : lifted) 

0 

(send owner : lifted name)) 



(defmethod (leg : new-f oothold) 

0 

(cond ( (car foothold-list) 
(send self :set-max) 
t) 

(t 

nil) ) ) 



(defmethod (leg :permitted-cell) 
(t-cell) 

(send owner : permit ted-ce 11 t-cell) ) 



(defmethod (leg :place-able) 

0 

; check plan state as well, as foothold for the leg 

(if (equal (send plan-machine : state-name) 'available-leg) 
self - 
nil) ) 



(defmethod (leg :pro jected-pos ) 

0 
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(send executor : get-desired-pos ) ) 



(defmethod (leg : select-foothold) 

() 

; out-list: ((max-foothold max-tkm) (foothold-list) (tkm-list)) 

(let* ( (H (send owner :get-H6) ) 

(inv-H (send owner :get-inv-H6) ) 

(body-trans-rate (send owner : get-body-trans-ratelO ) ) 
(body-rotate-rate (send owner : get-body-rotate-ratelO ) ) 
(estimated- support -plane 

(send owner : get-estimated-support-plane) ) 

(out-list 

(send foothold-finder : find-foothold 

H inv-H body-trans-rate body-rotate-rate 
estimated-support-plane) ) ) 

(setf foothold (first (first out-list) ) ) 

(setf reserved-foothold foothold) 

(setf tkm (second (first out-list))) 

(setf reserved-tkm tkm) 

(setf foothold-list (second out-list) ) 

(setf tkm-list (third out-list)))) 



(defmethod (leg : send-decision) 

(a-decision) 

(send plan-machine : send-decision a-decision) 

; ) 



/(defmethod (leg : send-decision -.after) 

; (a-decision) 

(if (equal a-decision 'place) 

(send executor : set-desired-pos foothold))) 



(defmethod (leg : send-exchange) 
(a-leg) 

(setf exchanged-leg a-leg)) 



(defmethod (leg : set-max) 

() 

(do ((footholds (cdr foothold-list) (cdr footholds)) 
(tkms (cdr tkm-list) (cdr tkms) ) 

(max-foothold (car foothold-list)) 

(max-tkm (car tkm-list) j 
(out-footholds) (out-tkms) ) 

((null footholds) 

(setf foothold max-foothold) 

(setf tkm max-tkm) , 

(setf foothold-list out-footholds) 

(setf ' tkm-list out-tkms)) 

(cond ( (> (car tkms) max-tkm) 

(setf max-foothold (car footholds)) 

(setf max-tkm (car tkms))) 

(t 

(setf out-footholds 

(cons (car footholds) out-footholds) ) 
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(setf out-tkms 

(cons (car tkms) out-tkms)))))) 



(defmethod (leg : stable-without-p) 

0 

(send owner : stable-without-p self)) 



(defmethod (leg : supporting) 

0 

(cond ((equal (send plan-machine :state-name) 'planned-contact) 
self) 

((equal (send plan-machine :state-name) ' eligible-to-lif t ) 
self) 

(t nil) ) 

) 



(defmethod (leg : support ing-p) 

0 

(cond ((equal (send control-machine :state-name) 'contact) 
self) 

((equal (send control-machine :state-name) 'support) 
self) 

(t nil) ) 

) 



(defmethod (leg : terrain-point ) 
(t-cell) 

(send owner : terrain-point t-cell) ) 



(defmethod (leg :TKM-limit) 

0 

(cond ( (null tkm) 
self) 

( (< tkm 0 .1) 
self) 

(t 

nil) ) ) 



(defmethod (leg : TKM-limit-p) 

0 

(cond ( (null tkm-p) 

self) ( 

( (< tkm-p 0.5) 
sel-f ) 

(t nil) ) ) 



(defmethod (leg : update-tkm) 

0 
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(let ( (body-trans-rate (send owner : get-body-t rans-ratelO ) ) 

(body-rotate-rate (send owner : get-body-rotate-ratelO ) ) 
(inv-H (send owner :get-inv-H10 ) ) ) 

(setf tkm (send t Ion-calculator :find-tkm 

(to-body-t ransf orm inv-H foothold) 
body-trans-rate body-rotate-rate) ) ) 

) 



(defmethod (leg : update -tkm-p) 

0 

(let ( (body-trans-rate-p (send owner : get-body-trans-ratel) ) 

(body-rotate-rate-p (send owner : get-body-rotate-ratel ) ) 
(inv-H-p (send owner :get-inv-Hl) ) ) 

(setf tkm-p (send tkm-calculator :find-tkm 

(to-body-transf orm inv-H-p foothold) 
body-trans-rate-p body-rotate-rate-p) ) ) 

) 



(defmethod (leg : with-f oothold) 

0 

(cond (reserved-foothold 

(setf foothold reserved-foothold) 

(setf tkm reserved-tkm) 

self) 

(t nil) ) ) 



( 
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;;; -*- Mode : Common-Lisp; Package : USER; Base: 10 



robot math library 



(defun arc-cos (s) 
(acos s) ) 



(defun col-mul (mat coll co!2) 

(let ( (sum 0 ) ) 

(dotimes (i 4) 

(setf sum ( + sum (* (aref mat i coll) (aref mat i col2) ) ) ) ) 
sum) ) 



(defun counting (a-list) 

(do ( (a-list a-list (cdr a-list) ) 
(i 0 (+ i 1) ) ) 

( (null a-list) i) ) ) 



(defun crossprod (vectl vect2) 

(let* ( (xl (first vectl) ) (x2 (first vect2) ) 

(yl (second vectl) ) (y2 (second vect2) ) 

(zl (third vectl)) (z2 (third vect2) ) 

(x (- (* yl z2) (* y2 zl) ) ) 

(y (- (* x2 zl) (* xl z2) ) ) 

(z (- (* xl y2) (* x2 yl) ) ) ) 

(list x y z) ) ) 



(defun delete-list (a-list b-list) ; delete a-list from b-list 
(do ( (deleting-list a-list (cdr deleting-list) ) 

(deleted-list b-list) ) 

( (null deleting-list) deleted-list) 

(setf deleted-list (remove (car deleting-list) 

deleted-list :test 'equal)))) 



(def macro dequeue (queue) 

' (progl (car , queue) 

(setf , queue (cdr ,queue)))) 



(defun dotprod (vectl vect2) 

; No dimension limitation ! ! ! 

(apply '+ (mapcar ' * vectl vect2))) 



(defmacro enqueue (queue-name element) 
; global s : queue-name 
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; Value of recover field of command is a list. 

; Two recover command is possible for one sampling-time. 

; structure of QUEUE : (first second third . . . last) 

' (setq , queue-name (nconc , queue-name (list , element)))) 



(defmacro empty-queue (queue) 
' (setq , queue ' ( ) ) ) 



(defun ident ( ) 

(make-array ' (4 4) : initial-content s 
'(( 1000 ) 

(0 10 0 ) 

(0 0 10 ) 

( 0001 )))) 



(defun magnitude (a-vector) 

(sqrt (dotprod a-vector a-vector) ) ) 



(defun magvect (const vect) 

; magvect = const * vect 

(mapcar #' (lambda (a-element) 
(* const a-element)) 
vect) ) 



(defun matrixadd (mtl mt2) 

(let ( (mt 3 (ident) ) ) 

(dotimes (i 4) 

(do times (j 4) 

(setf (aref mt3 i j) (+ (aref mtl i j) (aref mt2 i j))))) 

mt3) ) 



(defun matrixinv (mat ) 

(let ( (px (- (col-mul mat 0 

(py (- (col-mul mat 1 

(pz (- (col-mul mat 2 

(matrix (transpose mat 
(setf (aref matrix 3 0) 0) 
(setf (aref matrix 3 2) 0) 
(setf (aref matrix 0 3) px) 
(setf (aref matrix 2 3) pz) 
matrix) ) 



3) ) ) 

3) ) ) 

3) ) ) 

) ) 

(setf (aref matrix 3 1) 0) 

(setf (aref matrix 3 3) 1) 
(setf (aref matrix 1 3) py) 



(defun matri-xmult (mtl mt2) 

(let ((mat (make-array ' (4 4)))) ; it defines 0 through 3. (4 is not included) 

(dotimes (i 4) ; will repeat i-0, 1, 2, and 3. (not 4) 

(dotimes (j 4) 

(setf (aref mat i j) 0) ; initialize to zero 

(dotimes (k 4) 

(setf (aref mat i j) (+ (aref mat i j) (* (aref mtl i k) 
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mat) ) 



(aref mt2 k j) ) ) ) ) ) ) 



(defun nil-list (a-list ) 

(do ( (a-list a-list (cdr a-list) ) 
(not-nil nil) ) 

( (null a-list) (not not-nil) ) 
(if (car a-list) 

(setf not-nil t) ) ) ) 



(defun normalize-vector (a-vector) 

(let ( (m (magnitude a-vector) ) ) 

(if (< m 0.0000001) 

(list 000) 

(magvect (/ 1.0 m) a-vector)))) 



(defun orthogonalization (mt) 

; Gram-Schimit orthogonalization process 



(let* ( (mx 


(ident) ) 














(tx 


(aref mt 0 3)) (ty (aref mt 1 


3) ) 


(tz 


(aref 


mt 


2 


3) ) 


(xl 


(aref mt 0 0) ) (x2 (aref mt 0 


1) ) 


<x3 


(aref 


mt 


0 


2) ) 


<yi 


(aref mt 1 0)) (y2 (aref mt 1 


1) ) 


<y3 


(aref 


mt 


1 


2) ) 


(zl 


(aref mt 2 0)) (z2 (aref mt 2 


1) ) 


(z3 


(aref 


mt 


2 


2) ) 


(ml 


(magnitude (list xl yl zl))) 














(xl 


(/ xl ml) ) 














<yi 


(/ yl ml) ) 














(zl 


(/ zl ml) ) 














(a 


(dotprod (list xl yl zl) (list 


x2 


y2 


z2) ) ) 








<x2 


(- x2 (* a xl) ) ) 














<y2 


(- y2 (* a yl) ) ) 














(z2 


(- z2 (* a zl) ) ) 














(m2 


(magnitude (list x2 y2 z2) ) ) 














(x2 


(/ x2 m2) ) 














<y2 


( / y2 m2 ) ) 














(z2 


(/ z2 m2) ) ) 















(setf 


(aref 


mx 


0 


0) 


xl) 


(setf 


(aref 


mx 


0 


1) 


x2 ) 


(setf 


(aref 


mx 


0 


2) 


x3 ) 


(setf 


(aref 


mx 


1 


0) 


yi) 


(setf 


(aref 


mx 


1 


1) 


y2) 


(setf 


(aref 


mx 


1 


2) 


y3) 


(setf 


(aref 


mx 


2 


0) 


zl) 


(setf 


(aref 


mx 


2 


1) 


z2 ) 


(setf 


(aref 


mx 


2 


2) 


z3 ) 


(setf 


(aref 


mx 


0 


3) 


tx) 


(setf 


(aref 


mx 


1 


3) 


ty) 


(setf 


(aref 


mx 


2 


3) 


tz) 



mx) ) 



(defun plane-transform ( plane matrix ) 

; Transf ormed-Plane = Plane * Matrix 

; plane is defined as ( (a b c) d) . (a b c) is unit normal, d is -(distance) . 
(let* ( (new-a nil) 

(new-b nil) 

(new-c nil) ( 

(new-d nil) 

(old-unit-normal (car plane) ) 

(old-d (cadr plane) ) 

(old-a (first old-unit-normal) ) 

(old-b (second old-unit -normal) ) 

(old-c (third old-unit-normal) ) 

(mag nil) ) 

(setf new-a ( + (* old-a (aref matrix 00)) (* old-b (aref matrix 1 0)) 



math-t . lisp Thu Nov 29 11:32:16 1990 4 









(* 


old-c 


(a ref 


matrix 


2 


0) ) ) ) 












(setf 


new-b 


< + 


(* 


old-a 


(a ref 


matrix 


0 


1) ) <* 


old-b 


(aref 


matrix 


1 


1) ) 








(* 


old-c 


(aref 


matrix 


2 


1) ) ) ) 












(setf 


new-c 


< + 


(* 


old-a 


(aref 


matrix 


0 


2) ) <* 


old-b 


(aref 


matrix 


1 


2) ) 








(* 


old-c 


(aref 


matrix 


2 


2) ) ) ) 












(setf 


new-d 


( + 


(* 


old-a 


(aref 


matrix 


0 


3) ) (* 


old-b 


(aref 


matrix 


1 


3) ) 








(* 


old-c 


(aref 


matrix 


2 


3)) old-d) ) 











(setf mag (magnitude (list new-a new-b new-c) ) ) 

(if (< (abs mag) 0.0000001) 

(print "Error in PlaneTransf orm" ) 

(list (list (/ new-a mag) (/ new-b mag) (/ new-c mag)) 
(/ new-d mag) ) ) ) ) 



(defun plane-distance (plane velocity position) 

; Plane (X - Q)N = 0 , straight line X = P + tA. 

; t = ( Q - P ) N / ( AN ) if A is normalized then t is signed distance. 
; if t is infinitive then plane-distance returnes nil. 

; plane-distance returns t. 

(let* ((A (normalize-vector velocity)) 

(N (first plane) ) 

(dis (- (second plane))) 

(Q (magvect dis N) ) ; magvect = const * vector 

(P position) 

(Q__P (vectsub Q P) ) 

(AN (dotprod A N) ) 

(numerator (dotprod Q__P N) ) ) 

(if (< (abs AN) 0.0000001) ; no crossing 

nil ; returns nil 

(/ numerator AN) ) ) ) 



(defun plane-intersection (a-line a-plane) 

; a-line ( (direction) (point) ) X = P + tA. 

; a-plane ( (unit -normal) -dist) (X - Q) N = 0. 

(let* ( (velocity (normalize-vector (first a-line) ) ) 

(position (second a-line) ) 

(t-value (plane-distance a-plane velocity position) ) ) 
(if t-value 

(vectadd position (magvect t-value velocity) ) 
nil) ) ) ; no intersection 



(defun plane-normal-distance (a-plane a-point) 

; vector-type-plane (abed) 

; paul-type-point transpose (x y z 1) 

(let* ((unit-normal (first a-plane)) 

(dis (second a-plane) ) 

(vector-type-plane (reverse (cons dis (reverse « unit-normal) )) ) 
(paul-type-point (reverse (cons 1 (reverse a-point) ) ) ) ) 
(dotprod' vector-type-plane paul-type-point) ) ) 



(defun rotatemat (axis angle) ; array index starts from 0 not 1. 
; return rotatematrix angle : radian axis : x y or z 

(let ((mat (ident)) 
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(cosa (cos angle) ) 

(sina (sin angle))) 

(case axis 

(x-axis 



(setf 


(aref 


mat 


1 


1) 


cosa) 


(setf 


(aref 


mat 


1 


2) 


(- sina) ) 


(setf 


(aref 


mat 


2 


1) 


sina ) 


(setf 


(aref 


mat 


2 


2) 


cosa) ) 


(y-axis 


(setf 


(aref 


mat 


0 


0) 


cosa) 


(setf 


(aref 


mat 


0 


2) 


sina) 


(setf 


(aref 


mat 


2 


0) 


(- sina) 


) (setf 


(aref 


mat 


2 


2) 


cosa) ) 


( z-axis 


(setf 


(aref 


mat 


0 


0) 


cosa) 


(setf 


(aref 


mat 


0 


1) 


(- sina) ) 


(setf 


(aref 


mat 


1 


0) 


sina) 


(setf (aref mat 1 


1) 


cosa) ) ) 



mat)) ; returns this value. 



(defun to-body-transf orm (inv-H points-wrt-earth) 

; returns points-wrt-body 

(if (listp (first points-wrt-earth) ) ; test multi-points 

(do ( (points points-wrt-earth (cdr points) ) ; multi-points case 

(out-points nil) ) 

( (null points) (reverse out-points) ) 

(setf out-points (cons (transform inv-H (car points) ) out-points) ) ) 
(transform inv-H points-wrt-earth) ) ) ; single point case 



(defun to-earth-transf orm (H points-wrt-body) 

; returns points-wrt-earth 

(if (listp (first points-wrt-body) ) 

(do ( (points points-wrt-body (cdr points) ) 
(out -points nil) ) 

((null points) (reverse out-points)) 
(setf out-points (cons (transform H (car 
(transform H points-wrt-body) ) ) 



; test multi-points 
; multi-points case 



points) ) out-points) ) ) 
; single point case 



(defun transform (mat point) ; array index starts from 0 not 1. 
(let ( (x (car point) ) 



(y (cadr point) ) 

(z (if (caddr point) (caddr point) 0) ) ) 



(list ( + 


(* x (aref mat 0 0) ) 
(aref mat 0 3) ) 


(* 


y 


(aref 


mat 


0 


1) ) 


(* 


z 


(aref 


mat 


0 


2) ) 


( + 


(* x (aref mat 1 0) ) 
(aref mat 1 3) ) 


(* 


y 


(aref 


mat 


1 


1) ) 


(* 


z 


(aref 


mat 


1 


2) ) 


( + 


(* x (aref mat 2 0 ) ) 
(aref mat 2 3) ) ) ) ) 


(* 


y 


(aref 


mat 


2 


1) ) 


(* 


z 


(aref 


mat 


2 


2) ) 



(defun transmat (x y z) 

; returns translational marix 
(let ( (matrix (ident) ) ) 

(setf (aref matrix 0 3) x) 
(setf (aref matrix 1 3) y) 
(setf (arref matrix 2 3) z) 
matrix) ) 



(defun transpose (mat) 

(let ((matrix (make-array '(4 4)))) 
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(dotimes (i 4) 

(dotimes (j 4) 

(setf (aref matrix i j) (aref mat j i) ) ) ) 
matrix) ) 



(defun unit-crossprod (vectl 
; generate unitnormal vector 



(let 



( (xl 

(yi 

(zl 

(X 

(y 

(z 

(m 

(list (/ 



(first 

(second 

(third 



<- <* 
(- (* 
(- (* 
( sqrt 
x m) 



yi 

x2 
xl 
( + 
(/ 



vectl) ) 
vectl) ) 
vectl) ) 
z2 ) (* y2 

zl) (* xl z2) ) ) 
y2 ) (* x2 yl) ) ) 
(* xx) (* y y) 
y m) (/ z m) ) ) ) 



vect2 ) 

of vectl X vect2 
(x2 (first vect2 ) ) 
(y2 (second vect2)) 
(z2 (third vect2) ) 
zl) ) ) 



(* z z))))) 



(defun vectadd (vectl vect2) 
; vectsub = vectl + vect2 
/ no limit in dimension 
(mapcar '+ vectl vect2)) 



(defun vectsub (vectl vect2) 
; vectsub - vectl - vect2 
; no limit in dimension 
(mapcar ' - vectl vect2) ) 



f 
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;; Mode : Common-Lisp ; Base: 10 -*- 

*******************^ 



over lap- foot hold- finder definition 



t>nr*****ni 



[■*★**★**•*** 



(def flavor overlap-foothold-finder (adjacent-leg-numbers) 
(foothold-finder) 

: init able -instance -variables ) 



(defmethod (overlap-foothold-finder rinitti) 
(leg-name) 

(cond ((equal leg-name 'legl) 

(setf adjacent-leg-numbers ' (3) ) 
(setf sixteen-footholds 





' ( ( 9.0 4.3) (9.0 


3.3) 


( 9.0 2.3) 


( 9.0 


1.3) 




(8.0 4.3) (8.0 


3.3) 


(8.0 2.3) 


( 8.0 


1.3) 




(7.0 4.3) (7.0 


3.3) 


( 7.0 2.3) 


( 7.0 


1.3) 




( 6.0 4.3) ( 6.0 


3.3) 


(6.0 2.3) 


( 6.0 


1.3) 




( 5.0 4.3) ( 5.0 


3.3) 


( 5.0 2.3) 


( 5.0 


1.3) 




( 4.0 4.3) ( 4.0 


3.3) 


(4.0 2.3) 


( 4.0 


1.3) 




( 3.0 4.3) ( 3.0 


3.3) 


(3.0 2.3) 


( 3.0 


1.3) ) ) 


(setf 


four-lines 












' ( ( (0 0.3420 -0.9397) ( 


9.5 2.7339 


0) ) 






( (0 -0.3420 -0.9397) ( 


9.5 2.7339 


0) ) 






( (0 -0.3420 -0.9397) ( 


2.5 2.7339 


0) ) 






( (0 0.3420 -0.9397) ( 


2.5 2.7339 


0) ) ) ) ) 




( (equal 


leg-name 'leg2) 










(setf 


ad j a cent -leg-numbers 


' (4) ) 






(setf 


sixteen-f ootholds 












M ( 9.0 -4.3) (9.0 


-3.3) 


( 9.0 -2.3) 


( 9.0 


-1.3) 




( 8.0 -4.3) ( 8.0 


-3.3) 


( 8.0 -2.3) 


( 8.0 


-1.3) 




( 7.0 -4.3) ( 7.0 


-3.3) 


( 7.0 -2.3) 


( 7.0 


-1.3) 




( 6.0 -4.3) ( 6.0 


-3.3) 


( 6.0 -2.3) 


( 6.0 


-1.3) 




( 5.0 -4.3) ( 5.0 


-3.3) 


( 5.0 -2.3) 


( 5.0 


-1.3) 




( 4.0 -4.3) ( 4.0 


-3.3) 


( 4.0 -2.3) 


( 4.0 


-1.3) 




( 3.0 -4.3) ( 3.0 


-3.3) 


( 3.0 -2.3) 


( 3.0 


-1.3) ) ) 


(setf 


four-lines 












' ( ( (0 0.3420 -0.9397) ( 


9.5 -2.7339 


0) ) 






( (0 -0.3420 -0.9397) ( 


9.5 -2.7339 


0) ) 






( (0 -0.3420 -0.9397) ( 


2.5 -2.7339 


0) ) 






( (0 0.3420 -0.9397) ( 


2.5 -2.7339 


0) ) ) ) ) 




( (equal 


leg-name 'leg3) 










(setf 


ad j a cent -leg-numbers 


' (1 


5) ) 






(setf 


sixteen-f ootholds 












' ( ( 3.0 4.3) (3.0 


3.3) 


( 3.0 2.3) 


( 3.0 


1.3) 




(2.0 4.3) (2.0 


3.3) 


(2.0 2.3) 


( 2.0 


1.3) 




( 1.0 4.3) ( 1.0 


3.3) 


( 1.0 2.3) 


( 1.0 


1.3) 




(0.0 4.3) (0.0 


3.3) 


( 0.0 2.3) 


( 0.0 


1.3) 




(-1.0 4.3) (-1.0 


3.3) 


(-1.0 2.3) 


(-1.0 


1.3) 




(-2.0 4.3) (-2.0 


3.3) 


(-2.0 2.3) 


(-2.0 


1.3) 




(-3.0 4.3) (-3.0 


3.3) 


(-3.0 2.3) 


(-3.0 


1.3) ) ) 


(setf 


four-lines t 












' ( ( (0 0.3420 -0.9397) ( 


3.5 2.7339 


0) ) 




- 


( (0 -0.3420 -0.9397) ( 


3.5 2.7339 


0) ) 






( (0 -0.3420 -0.9397) (- 


3.5 2.7339 


0) ) 






( (0 0.3420 -0.9397) (- 


3.5 2.7339 


0))))) 




( (equal 


. leg-name 'leg4) 










(setf 


ad j a cent -leg-numbers 


' (2 


6) ) 






(setf 


sixteen-footholds 












' ( ( 3.0 -4.3) (3.0 


-3. 


3) ( 3.0 -2 


.3) ( 3 


.0 -1. 
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( 2.0 


-4.3) 


( 2.0 


-3.3) 


( 2.0 


-2.3) 


( 2.0 


-1.3) 


( 1.0 


-4.3) 


( 1.0 


-3.3) 


( 1.0 


-2.3) 


( 1.0 


-1.3) 


( 0.0 


-4.3) 


( 0.0 


-3.3) 


( 0.0 


-2.3) 


( 0.0 


-1.3) 


(-1.0 


-4.3) 


(-1.0 


-3.3) 


(-1.0 


-2.3) 


(-1.0 


-1.3) 


(-2 . 0 


-4.3) 


(-2.0 


-3.3) 


(-2.0 


-2.3) 


(-2.0 


-1.3) 


(-3.0 


-4.3) 


(-3.0 


-3.3) 


(-3.0 


-2.3) 


(-3.0 


-1.3) ) ) 



(setf four-lines 

'(((0 0.3420 -0.9397) ( 3.5 -2.7339 0)) 

((0 -0.3420 -0.9397) ( 3.5 -2.7339 0)) 

((0 -0.3420 -0.9397) (-3.5 -2.7339 0)) 

((0 0.3420 -0.9397) (-3.5 -2.7339 0))))) 

((equal leg-name f leg5) 

(setf ad jacent-leg-numbers ' (3) ) 

(setf sixteen-footholds 



' ( (-3.0 


4.3) 


(-3.0 


3.3) 


(-3.0 


2.3) 


(-3.0 


1.3) 


(-4.0 


4.3) 


(-4.0 


3.3) 


(-4.0 


2.3) 


(-4.0 


1.3) 


(-5.0 


4.3) 


(-5.0 


3.3) 


(-5.0 


2.3) 


(-5.0 


1.3) 


(-6.0 


4.3) 


(-6.0 


3.3) 


(-6.0 


2.3) 


(-6.0 


1.3) 


(-7.0 


4.3) 


(-7.0 


3.3) 


(-7.0 


2.3) 


(-7.0 


1.3) 


(-8.0 


4.3) 


(-8.0 


3.3) 


(-8.0 


2.3) 


(-8.0 


1.3) 


(-9.0 


4.3) 


(-9.0 


3.3) 


(-9.0 


2.3) 


(-9.0 


1.3) ) ) 


four-lines 














' ( ( (0 0 , 


.3420 


-0.9397) (-2 


.5 2 


.7339 


0) ) 




((0 - 0 , 


.3420 


-0.9397) (-2 


.5 2 


.7339 


0) ) 




( (0 -0, 


.3420 


-0.9397) (-9 


.5 2 


.7339 


0) ) 




( (0 0 , 


.3420 


-0.9397) (-9 


.5 2 


.7339 


0) ) ) ) ) 





((equal leg-name f leg6) 

(setf adjacent-leg-numbers '(4)) 
(setf sixteen-f ootholds 



' ( (-3.0 


-4.3) 


(-3.0 -3.3) (-3.0 


-2.3) 


(-3.0 -1.3) 


(-4.0 


-4.3) 


(-4.0 -3.3) (-4.0 


-2.3) 


(-4.0 -1.3) 


(-5.0 


-4.3) 


(-5.0 -3.3) (-5.0 


-2.3) 


(-5.0 -1.3) 


(-6.0 


-4.3) 


(-6.0 -3.3) (-6.0 


-2.3) 


(-6.0 -1.3) 


(-7.0 


-4.3) 


(-7.0 -3.3) (-7.0 


-2.3) 


(-7.0 -1.3) 


(-8.0 


-4.3) 


(-8.0 -3.3) (-8.0 


-2.3) 


(-8.0 -1.3) 


(-9.0 


-4.3) 


(-9.0 -3.3) (-9.0 


-2.3) 


(-9.0 -1.3) ) ) 


(setf four-lines 








' ( ( (0 0 


.3420 


-0.9397) (-2.5 -2, 


.7339 


0) ) 


( (0 -0 


.3420 


-0.9397) (-2.5 -2, 


.7339 


0) ) 


( (0 -0 


.3420 


-0.9397) (-9.5 -2, 


.7339 


0) ) 


( (0 0 


.3420 


-0.9397) (-9.5 -2, 


.7339 


0) ) ) ) ) 


) 

(setf tkm-calculator 


(send 


owner : tkm-calculator ) ) 





(defmethod (overlap-foothold-finder : get-possible-footholds) 
(estimated-footholds H inv-H) 

; returns possible-f ootholds wrt body 
; find-possible-footholds function tests obstacles 
(to-body-transf orm 

inv-H ( 

(send self : get-rid-of -overlap 

(send self : find-possible-footholds 

(to-earth-transf orm H estimated-footholds))))) 



(defmethod (overlap-foothold-finder : get-rid-of-overlap) 
( footholds -wrt -earth) 
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(let* ( (a jacent-legs 
(mapcar 

#' (lambda (leg-num) 

(•send owner :nth-leg leg-num) ) 
adjacent-leg-numbers) ) 

(ad j a cent -legs -in-possible- interact ion 
( remove 
nil 

(mapcar #' (lambda (leg) 

(if (send leg :place-able) ;no interaction 
nil 
leg) ) 

a jacent-legs) ) ) ) 

(send self : remove-overlapped-f oothold 
f oot holds -wrt -earth 

adjacent-legs-in-possible-interaction) ) ) 



(defmethod (overlap-foothold-finder : remove-overlapped-f oothold. 

(footholds -wrt -earth legs -in-possible -interact ion) 

(do ((legs legs-in-possible-interaction (cdr legs)) 

(out -foot ho Ids footholds -wrt -earth) 

(overlap-foothold) ) 

( (null legs) out-footholds) 

(setf overlap-foothold (send (car legs) : foothold)) 

(setf out-footholds 

(remove overlap-foothold 
out-footholds 
:test #' (lambda (xl x2) 

(send self :overlap-p xl x2) ) 

) ) ) ) 



(defmethod (overlap-foothold-finder :overlap-p) 
(xl x2) 

(let ( (xl-integer (mapcar #' truncate xl) ) 
(x2-integer (mapcar #' truncate x2) ) ) 
(equal xl-integer x2-integer) ) ) 



i 
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;;; Mode : Common-Lisp; Base: 10 

/ 

; overlap-leg definition 
/ 



(defflavor overlap-leg 
(leg) 



) 



0 



(defmethod (overlap-leg :initti) 
(H) 

(setf contact-sensor 
(setf executor 
(setf control-machine 
(setf plan-machine 
(setf tkm-calculator 
(setf foothold-finder 



(make-instance 'contact-sensor 
(make-instance 'executor 



: owner self)) 

: owner self) ) 

(make-instance 'control-state-machine :owner self)) 
(make-instance 'plan-state-machine :owner self)) 
(make-instance ' overlap-tkm-calculator : owner self)) 
(make-instance 'overlap-foothold-finder :owner self)) 



(setf foothold (send executor : initti name H) ) 
(send contact-sensor : initti name) 

(send control-machine : initti name) 

(send plan-machine : initti name) 

(send tkm-calculator : initti name) 

(send foothold-finder : initti name)) 



(defmethod (overlap-leg :nth-leg) 
(leg-num) 

(send owner :nth-leg leg-num) ) 



(defmethod (overlap-leg : foothold) 

0 

foothold) 



i 
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;; Mode : Common-Lisp ; Base: 10 -*- 

overlap-robot definition 



(def flavor overlap-robot ( ) 
(robot) 

: initable-instance-variables 
: ge t table -ins t ance - va r iab le s ) 



(defmethod (overlap-robot rinitti) 

0 

(send graph-asv :init-data) 

(setf vision-system (make-instance 'vision-system rowner self)) 
(send vision-system :initti) 

(setf joystick (make-instance ' joystick) ) 

(send joystick :reset) 

(empty-queue lift-queue) 

(setf lift-flag t) 

(let ((H)) 

(setf body (make-instance 'body :owner self)) 

(setf H (send body rinitti)) 

(setf legs (list 



(make -in stance 


' overlap-leg 


•.name 


' legl 


: owner 


self) 


(make -in st ance 


' overlap-leg 


: name 


' leg2 


r owner 


self) 


(make-instance 


' overlap-leg 


r name 


' leg3 


rowner 


self) 


(make -in st ance 


' overlap-leg 


r name 


' leg4 


: owner 


self) 


(make -in st ance 


' overlap-leg 


rname 


' leg5 


: owner 


self) 


(make- instance 


' overlap-leg 


: name 


' leg6 


: owner 


self ) 



) ) 

(mapcar #' (lambda (a-leg) (send a-leg rinitti H) ) legs)) 

) 



(defmethod (overlap-robot : nth-leg) 
(leg-num) 

; nth starts counting from zero. 

; leg-num starts from one. 

(nth (- leg-num 1) legs) ) 



t 
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;; Mode : Common- Lisp; Base:10 -*- 

★ **★★★***★★★★**★★★**★★★★★★★★★★*★**★★*★★★**★★★★★★★★★★**•★★****•*★★★*★* 

overlap-tkm-calculator definition 



(def flavor overlap-tkm-calculator ( ) 
(tkm-calculator) 

: initable-instance-variables) 



(def met hod (overlap-tkm-calculator 
(leg-name) 

(cond ((equal leg-name 'legl) 
(setf working-volume 



: initt i) 





' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-9.5) 


( (0 


0 . 9397 


0 ^420) 


-2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


-2.5) 


( (0 


0.9397 


-0 . 420 ) 


-2.569))))) 


( (equal 


leg-name 'leg2) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-9.5) 


( (0 


0.9397 


0.3420) 


2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


-2.5) 


( (0 


0.9397 


-0.3420) 


2.569) ) ) ) ) 


( (equal 


leg-name 'leg3) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-3.5) 


( (0 


0.9397 


0.3420) 


-2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


3.5) 


( (0 


0.9397 


-0.3420) 


-2.569) ) ) ) ) 


( (equal 


leg-name 'leg4) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-3.5) 


( (0 


0.9397 


0.3420) 


2.569) ) 




(((0 0 1) 5.7313) 


( (1 


0 


0) 


3.5) 


( (0 


0.9397 


-0.3420) 


2.569))))) 


( (equal 


leg-name f leg5) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


2.5) 


( (0 


0.9397 


0.3420) 


-2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


9.5) 


( (0 


0.9397 


-0.3420) 


-2.569) ) ) ) ) 


( (equal 


leg-name 'leg6) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


2.5) 


( (0 


0.9397 


0.3420) 


2.569) ) 




(((0 0 1) 5.7313) 


( (1 


0 


0) 


9.5) 


( (0 


0.9397 


-0.3420) 


2.569) ) ) ) ) 
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;;; Mode : Common-Lisp ; Base: 10 -*- 



plan-state flavor definition 

A******************************************************* 



(defflavor plan-state ( (decision nil) (observation nil) 

(condition nil) ) 



(state) 

: initable-instance-variables) 



(command nil) 



(defmethod (plan-state : generate-command) 

0 

command) 



(defmethod (plan-state : change) 

(given-decision observed-state given-condition) 
(cond ( (and decision (listp decision) ) 

(cond ( (equal given-decision (first decision) ) 
(first next-state) ) 

( (equal given-decision (second decision) ) 
(second next-state) ) 

(t self) ) ) 

(condition 

(if (and (equal given-conc.ition condition) 

(equal observed-state observation) ) 
next-state 
self) ) 

(t 

(cond ( (equal observed-state observation) 
next-state) 

( (equal given-decision decision) 
next-state) 

(t self) ) ) ) ) 



plan-state-machinie flavor definition 

★ ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★it 



(defflavor plan-state-machine ( (decision nil) (observation nil) 

(condition nil) (lift-ready-flag 
, control-machine) 



(state-machine) 

: initable-instance-variables) 



nil) 



(defmethod (plan-state-machine rinitti) 
(leg-name) 

(if (member leg-name 9 (legl leg4 leg5) ) 
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(send self : init-plan-machine ' eligible-to-lif t ) 
(send self : init-plan-machine 'available-leg)) 
(setf control-machine (send owner : control-machine) ) ) 



(defmethod (plan-state-machine : init-plan-machine) 

(a -st ate -name) 

(let (available-leg planned-contact eligible-to-lif t 
planned-lift actual-lift planned-exchange) 

(setf actual-lift 

(make-instance 'plan-state 

:name 'actual-lift 
: observation 'ready 
: command 'recover-command)) 

(setf planned-lift 

(make-instance 'plan-state 

:name 'planned-lift : condition ' stable-without 
: observation 'support 
:next-state actual-lift)) 

(setf planned-exchange 

(make-instance 'plan-state 

:name 'planned-exchange : condition 'interlock-confirm 
.•observation 'support 
rnext-state actual-lift)) 

(setf eligible-to-lif t 

(make-instance 'plan-state 

: name ' eligible-to-lif t 
•.decision ' (lift exchange) 

:next-state (list planned-lift planned-exchange))) 

(setf planned-contact 

(make-instance 'plan-state 

:name 'planned-contact : observation 'contact 
: command 'deploy-command 
:next-state eligible-to-lif t ) ) 

(setf available-leg 

(make-instance 'plan-state 

:name 'available-leg : decision 'place 
rnext-state planned-contact)) 

(send actual-lift : set -next-state available-leg) 



(setf state (cond 



) 

) 



( (equal a-state-name 
available-leg) 

( (equal a-state-name 
planned-contact ) 

( (equal a-state-name 
eligible-to-lif t) 

( (equal a-state-name 
planned-lift ) 

( (equal a-state-name 
planned-exchange) 

( (equal a-state-name 
actual-lift ) ) ) 



(send available-leg : state-name) ) 
(send planned-contact : state-name) ) 
(send eligible-to-lif t : state-name) ) 
(send planned-lift : state-name) ) 
(send planned-exchange : state-name) ) 
(send actual-lift : state-name) ) 



i 



(defmethod (plan-state-machine : change :before) 

0 

(setf observation (send control-machine : state-name) ) 

(cond ((and (equal (send state : state-name) 'planned-exchange) 
(send owner : interlock-confirm) 

(send owner : stable-without-p) 
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(send owner :lift-ok)) 

(setf lif t-ready-f lag t) 

(setf condition 'interlock-confirm)) 

((and (equal (send state :state-name) 'planned-lift) 
(send owner : stable-without-p) 

(send owner :lift-ok)) 

(setf lift-ready-flag t) 

(setf condition ' stable-without ) ) 

(t 

(setf condition nil) 

(setf lift-ready-flag nil) ) ) 



(defmethod (plan-state-machine rchange) 

() 

(setf observation (send control-machine : state-name ) ) 

(cond ((and (equal (send state : state-name) 'planned-exchange) 
(send owner : interlock-confirm) 

(send owner : stable-without-p) 

(send owner :lift-ok)) 

(setf lift-ready-flag t) 

(setf condition ' interlock-conf inn) ) 

((and (equal (send state :state-name) 'planned-lift) 

(send owner : stable-without-p) 

(send owner :lift-ok)) 

(setf lift-ready-flag t) 

(setf condition 'stable-without)) 

(t 

(setf condition nil) 

(setf lift-ready-flag nil) ) ) 

(setf state (send state :change decision observation condition) ) 

; ) 

; (defmethod (plan-state-machine : change : after) 

; 0 

(send control-machine : send-command 

(send state : generate-command) ) 

(if (and lift-ready-flag 

(equal (send self :state-name) 'actual-lift)) 

(send owner : lifted))) 



(defmethod (plan-state-machine : send-decision) 
(a-decision) 

(setf decision a-decision) ) 
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... Mode : Common-Lisp; Base: 10 - 



; robot flavor definition 

.*★★★*★**★******★★*★★**★*★★*★★***★**★****★***★********★** 

/ 



(def flavor robot (legs body vision-system joystick 
( lif t-able-legs nil) 

(place-able-legs nil) (supporting-legs nil) 
(supporting-p-legs nil) 

(joy-command '(0 0 0)) lift-queue lift-flag) 

0 

: initable-instance-variables 
:gettable-instance-variables) 



(defmethod (robot :initti) 

0 

(send graph-asv :init-data) 

(setf vision-system (make-instance 'vision-system towner self)) 
(send vision-system :initti) 

(setf joystick (make-instance ' joystick) ) 

(send joystick :reset) 

(empty-queue lift-queue) 

(setf lift-flag t) 

(let ((H)) 

(setf body (make-instance 'body :owner self)) 

(setf H (send body :initti)) 

(setf legs (list 



(make -instance 


' leg 


: name 


' legl 


: owner 


self) 


(make-instance 


'leg 


: name 


' leg2 


: owner 


self) 


(make-instance 


' leg 


: name 


' leg3 


: owner 


self) 


(make -instance 


' leg 


:name 


' leg4 


: owner 


self) 


(make -instance 


'leg 


: name 


'leg5 


: owner 


self) 


(make-instance 


'leg 


:name 


' leg6 


: owner 


self) 



) ) 

(mapcar #' (lambda (a-leg) (send a-leg :initti H) ) legs)) 



(defmethod (robot : find-lif t-able-legs) 

0 

(delete nil (mapcar #' (lambda (a-leg) (send a-leg :lift-able)) legs))) 



(defmethod (robot : f ind-place-able-legs) 

0 

(delete nil (mapcar #' (lambda (a-leg) (send a-leg :place-able) ) legs))) 

i 



(defmethod (robot ; find-supporting-legs) 

0 

(delete nil (mapcar #' (lambda (a-leg) (send a-leg -.supporting)) legs))) 
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(defmethod (robot : f ind-supporting-p-legs ) 

0 

(delete nil (mapcar #' (lambda (a-leg) (send a-leg : support ing-p) ) legs))) 



(defmethod (robot : get-body-rotate-ratel ) 
0 

(send body : get-body-rotate-ratel) ) 



(defmethod (robot : get-body-rotate-ratelO ) 
0 

(send body : get-body-rotate-ratelO ) ) 



(defmethod (robot : get-body-t rans-ratel ) 
0 

(send body : get-body-trans-ratel) ) 



(defmethod (robot : get-body-trans-ratelO ) 

0 

(send body : get-body-t rans-ratelO ) ) 



(defmethod (robot : get-estimated-support-plane) 
0 

(send body : get-estimated-support-plane ) ) 



(defmethod (robot :get-Hl) 
0 

(send body :get-Hl) ) 



(defmethod (robot :get-H6) 

0 

(send body :get-H6) ) 



(defmethod (robot :get-H10) 

0 

(send body :get-H10) ) 



(defmethod (robot :get-inv-Hl) 

0 i 

(send body :get-inv-Hl) ) 



(defmethod (robot :get-inv-H6) 
0 

(send body :get-inv-H6) ) 
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(de fmethod (robot : get-inv-HIO ) 
0 

(send body : get-inv-HIO ) ) 



(defmethod (robot :lift-ok.) 

(leg-name) 

(cond (lift-flag 

(cond ((equal leg-name (send (first lift-queue) :name) ) 
(setf lift-flag nil) 
t) 

(t 

nil) ) ) 

(t nil) ) ) 



(defmethod (robot : lifted) 

(leg-name) 

(if (equal leg-name (send (first lift-queue) :name) ) 
(dequeue lift-queue) 

(print (list "error in lifting" leg-name) ) ) ) 



(defmethod (robot :permitted-cell) 

(t-cell) 

(send vision-system :permitted-cell t-cell) ) 



(defmethod (robot : scanning) 

0 

(send vision-system : scanning)) 



(defmethod (robot : stable-without-p) 

(a-leg) 

(send body :stable-p 

(remove a-leg support ing-p-legs) ) ) 



(defmethod (robot : terrain-point ) 

(t-cell) 

(send vision-system : terrain-point t-cell) ) 



•★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★it**** 

' ( 

; prolog interface robot methods 

9 

.********★************************************************** 
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(defmethod (robot : at-tkm-limit ) 

0 

(let ( (limit-leg 

(car (delete nil 

(mapcar #' (lambda (a-leg) (send a-leg :TKM-limit)) lif t-able-legs) ) ) ) ) 
(setf supporting-legs (remove limit-leg 

supporting-legs) ) 

(setf lif t-able-legs (remove limit-leg 

lif t-able-legs) ) 

limit-leg) ) 



(defmethod (robot : check-stability-p) 

0 

(send body :stable-p-m supporting-p-legs (first lift-queue))) 



(defmethod (robot : check-tkm-limit-p) 

0 

(delete nil 

(mapcar #' (lambda (a-leg) (send a-leg : TKM-limit-p) ) supporting-p-legs))) 



(defmethod (robot : do-recovery ) 

0 

(car 

(delete nil 

(mapcar #' (lambda (a-leg) (send a-leg : with-f oothold) ) place-able-legs) ) ) 



(defmethod (robot : execute-planned-motion) 

0 

(mapcar #' (lambda (a-leg) (send a-leg : do-planned-motion) ) legs)) 



(defmethod (robot : graphical-display) 

0 

(send graph-asv rdisplay (send body :get-Hl) 

(mapcar #' (lambda (a-leg) (send a-leg : leg-pos-wrt-body) ) legs)) 

) 



(defmethod (robot : has-more-tkm) 
(legl leg2) 

(> (send legl :tkm) 

(send leg2 :tkm) ) ) 



(defmethod (robot : leg-with-new-f oothold) 

(> 

; return a-leg with new-f oothold. 

(do ( (new-foothold-f lags (mapcar #' (lambda (a-leg) (send a-leg : new-f oothold) ) plac 
e-legs) 

e-legs) ) 

(a-leg nil) ) 



(mapcar #' (lambda (a-leg) (send a-leg : new-f oothold) ) plac 
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((or (nil-list new-f oothold-f lags ) 
a-leg) 

(if a-leg a-leg nil)) 

(setf a-leg (send self :max-sm-leg nil) ) ) ) 



(def method (robot :max-sm-leg) 

(a-leg) 

; max-sm-leg without supporting a-leg 
(let (legs-with-f oothold) 

(cond (place-able-legs 

(setf legs-with-f oothold 

(remove nil (mapcar #' (lambda (leg) 

(if (send leg : has-f oothold-p) 
leg 
nil) ) 

place-able-legs) ) ) 

(cond (legs-with-f oothold 

(do ( (legs (cdr legs-with-f oothold) (cdr legs) ) 

(largest-leg (car legs-with-f oothold) largest-leg) 
(temp-support-legs (remove a-leg supporting-legs) ) ) 

( (null legs) 

(if (send body :stable (cons largest-leg temp-support-legs)) 
largest-leg 
nil) ) 

(if (send body :more-stable temp-support-legs 
(car legs) largest-leg) 

(setf largest-leg (car legs))))) 

(t nil) ) ) 

(t nil) ) ) ) 



(defmethod (robot :modif y-command) 

0 

(send body : modi f y-command) ) 



(defmethod (robot : wait-f or-lif t ) 

0 

(delete nil 

(mapcar #' (lambda (a-leg) (send a-leg : lif t-not-done ) ) support ing-p-legs ) ) ) 



(defmethod (robot : read- joystick) 

0 

(let ((joy-value (send joystick : get- joy-value) ) ) 

(setf joy-command 

(reverse (cdr (reverse (send joystick :get- joy-value) ))) ) 
(if (fourth joy-value) 
nil 
t) ) ) 



( 

(defmethod (xobot : restore-command) 

0 

(send body : restore-command) ) 



(defmethod (robot : before : send-decision) 
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(legl leg2 a-decision) 
(cond ((equal a-decision 'exchange) 
(enqueue lift-queue legl) ) 

( (equal a-decision ' lift) 
(enqueue lift-queue legl)))) 



(defmethod (robot : send-decision) 

(legl leg2 a-decision) 

(cond ((equal a-decision 'exchange) 

(send legl : send-decision a-decision) 
(send leg2 : send-decision 'place) 

(send legl : send-exchange leg2) ) 

(t 

(send legl : send-decision a-decision)))) 



(defmethod (robot : smallest-tkm-leg) 

0 

; select smallest-TKM-leg 
; tkin is nil or positive 

(do ( (legs (cdr lif t-able-legs) (cdr legs) ) 

(smallest-leg (car lif t-able-legs) ) 

(smallest-tkjn nil) (tkm nil) ) 

( (null legs) smallest-leg) 

(setf smallest-tkm (if (send smallest-leg :tkm) 

(send smallest-leg :tkm) -1000)) 

(setf tkm (if (send (car legs) :tkm) 

(send (car legs) :tkm) -1000)) 

(if (> smallest-tkm tkm) (setf smallest-leg (car legs) ) ) 

(if (and (equal smallest-tkm -1000) (equal tkm -1000)) 

"Error : more than one legs are out of kinematic limit") ) ) 



(defmethod (robot : slow-down-robot ) 

0 

(send body : slow-down)) 



(defmethod (robot : speed-up-robot ) 

0 

(send body : speed-up)) 



(defmethod (robot : stable) 

0 

(send body : stable supporting-legs) ) 



(defmethod (robot :stable_m) 

0 

(send body :stable-m supporting-legs)) 



(defmethod (robot : stable-without) 

(a-leg) 

(send body rstable (remove a-leg supporting-legs))) 
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(defmethod (robot : update-robot-status ) 

0 

(setf lift-flag't) 

(setf lif t-able-legs (send self : f ind-lif t-able-legs) ) 

(setf place-able-legs (send self : f ind-place-able-legs ) ) 

(setf supporting-legs (send self : f ind-supporting-legs) ) 

(setf supporting-p-legs (send self : f ind-supporting-p-legs) ) 

(mapcar #' (lambda (a-leg) (send a-leg : update-tkm-p) ) supporting-p-legs) 
(if (send self : check -t km- limit -p) 

(send body :stop-motion (send self : check-tkm-limit-p) ) 

(send body : restore-motion) ) 

(if (not (send self : check-stability-p) ) 

(send body :modif y-command-p) 

(send body : restore-command-p) ) 

(send body : calculate-motion joy-command legs) 

(mapcar #' (lambda (a-leg) (send a-leg : select-foothold) ) place-able-legs) 
(mapcar #' (lambda (a-leg) (send a-leg :update-tkm) ) supporting-legs) ) 



(defun create-terrain 
(send graph-terrain 



0 

: create) ) 



(defun kill-terrain ( ) 

(send graph-terrain :kill) ) 



★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★a******* 

Prolog interface functions 

**r ****************************************************** 



(defun at__tkm__limit ( ) 

(send asv : at-tkm-limit ) ) 



(defun do_recovery ( ) 

(send asv : do-recovery) ) 



(defun exe cu t e_p 1 a nne demotion ( ) 

(send asv : execute-planned-motion) ) 



(defun graphical__display () ( 

(send asv : graphical-display) ) 



(defun has__more__tkm(legl leg2) 

(send asv :has-more-tkm legl leg2)) 
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(defun initsO 

(send asv :initti)) 



(defun leg_with_new_f oothold ( ) 

(send asv : leg-with-new-f oothold) ) 



(defun max_sm_leg ( a-leg) 

(send asv :max-sm-leg a-leg) ) 



(defun modif y^command { ) 

(send asv : modif y-command) ) 



(defun read__joystick ( ) 

(send asv : read- joystick) ) 



(defun restore_command() 

(send asv : restore-command) ) 



(defun send_decision ( legl leg2 a-decision) 

(send asv : send-decision legl leg2 a-decision)) 



(defun smallest_tkm_leg ( ) 

(send asv : smallest -tkm-leg) ) 



(defun slow_down_robot ( ) 

(send asv : slow-down-robot ) ) 



(defun speed_up_robot ( ) 

(send asv : speed-up-robot ) ) 



(defun stable_p() 

(send asv :stable)) 



i 

(defun stable__p_m ( ) 

(send asv " : stable_m) ) 



(defun stable_without (a-leg) 

(send asv : stable-without a-leg)) 
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(defun update_robot_status ( ) 

(send asv :update“robot-status ) ) 



1 



robot-t ranslation . lisp Thu Nov 29 13:11:55 1990 

(fs : add-logical-pathname-host "robot” "root5" 

' ( ("kwak" "supermac : kwak : ” ) ) ) 



1 



robot “translation . lisp Thu Nov 29 13 : 11:55 1990 

(f s : add“logical“pathname“host "robot” "root5" 

' ( ( "kwak" "supermac : kwak : " ) ) ) 
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;; Mode : Common-Lisp; Package : USER; Base: 10 

******************************************************************* 

top level motion planning coordinator 
*******★★★**★★★★****★*★★***■*■**★**★**★★*****★****★**★****★**★★**★★★* 



(defun my-monitor (&rest args) 

(let ( (x (mapcar #'my-output args))) 

(if (remove nil x) 

(my-print x) ) ) 
t) 

(defun my-output (arg) 

(cond ( (typep arg 'leg) (send arg :name) ) 

( (typep arg 'atom) arg) 

((typep arg 'list) (cons (my-output (car arg)) 

(my-output (cdr arg) ) ) ) 

(t ' error) ) ) 



(defmacro retract (predicate &optional (argument t) ) 

' (cond ((not (boundp (quote , predicate) ) ) 
nil) 

((and , predicate (equal , argument '?)) 

(setf , predicate (cdr , predicate)) 
t) 

((member , argument , predicate :test 'equal) 

(setf , predicate (remove , argument ,predicate icount 1) ) 
t) 

(t nil))) 



(defmacro asserta (predicate &optional (argument t) ) 

' (cond ((not (boundp (quote , predicate) ) ) 

(setf , predicate nil) 

(setf , predicate (cons , argument , predicate) ) ) 

(t (setf , predicate (cons , argument , predicate) ))) ) 



(defmacro assertz (predicate ^optional (argument t) ) 

'(cond ((not (boundp (quote , predicate )) ) 

(setf , predicate nil) 

(setf , predicate (append , predicate (list , argument ))) ) 

(t (setf , predicate (append , predicate (list , argument ))))) ) 



(defmacro match (predicate &optional (argument t) ) 

' (cond ( (not (boundp (quote , predicate) ) ) 
nil) 

((member , argument , predicate rtest 'equal) 
t) 

(t nil) ) ) 



(defmacro unify (predicate argument) 

' (cond ( (not (boundp (quote , predicate) ) ) 
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;; Mode : Common-Lisp; Package :USER; Base: 10 

******************************************************************* 

top level motion planning coordinator 



(defun my-monitor (&rest args) 

(let ( (x (mapcar #'my-output args))) 

(if (remove nil x) 

(my-print x) ) ) 
t) 

(defun my-output (arg) 

(cond ( (typep arg 'leg) (send arg :name) ) 

( (typep arg 'atom) arg) 

((typep arg 'list) (cons (my-output (car arg)) 

(my-output (cdr arg) ) ) ) 

(t ' error) ) ) 



(defmacro retract (predicate ^optional (argument t) ) 

' (cond ({not (boundp (quote , predicate) ) ) 
nil) 

((and predicate (equal , argument '?)) 

(setf , predicate (cdr , predicate)) 
t) 

((member , argument , predicate :test 'equal) 

(setf , predicate (remove , argument , predicate :count 1) ) 
t) 

(t nil) ) ) 



(defmacro asserta (predicate ^optional (argument t) ) 

' (cond ((not (boundp (quote /predicate))) 

(setf /predicate nil) 

(setf /predicate (cons /argument /predicate))) 

(t (setf /predicate (cons /argument /predicate))))) 



(defmacro assertz (predicate fioptional (argument t) ) 

' (cond ((not (boundp (quote /predicate))) 

(setf /predicate nil) 

(setf /predicate (append /predicate (list /argument)))) 

(t (setf /predicate (append /predicate (list /argument) ) ) ) ) ) 



(defmacro match (predicate ^optional (argument t) ) 
'(cond ((not (boundp (quote /predicate))) 
nil) 

((member /argument /predicate :test 'equal) 
t) 

(t nil) ) ) 



(defmacro unify (predicate argument) 

'(cond ((not (boundp (quote /predicate))) 
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nil) 

(t (setf /argument (car /predicate))))) 



; robot initialize, repeat, my_loop, fail. 

; initialize inits, init__ditch_plan . 

/ init_dicth_ plan retract (plan__cycle (__)) , retract (plan_state (_) ) , fail. 

; init_ditch_plan asserta (plan__cycle ( 1 )) , asserta (plan__state (place_legs_in_the_air) 

; my__loop get_cornmand, plan, execute, !. 

; get_command X is read_joyst ick . 

; plan ditch_mode, ditch_plan. 

; plan normal_plan. 

; ditch_mode ditch_mode ( in) . ;; cleared by ditch_plan. 

; ditch_mode : - X is at_ditch__area, X == t, asserta (ditch__mode (in) ) . 

; execute execute_mot ion, draw_robot, !. 

; execute_motion X is execute_planned_motion . 

; draw_robot X is graphical_display . 



(defun robot () 

(create-terrain) 
( robot 1) 
(kill-terrain) ) 



(defun robot 1 () 

(initialize) 

(do () 

( (not (my_loop) ) ) ) ) 



(defun initialize () 

(cond ((and (inits) 

(init_ditch_plan) ) 
t) 

(t nil))) 



(defun init_ditch_plan ( ) 

(cond ((and (not (setf ditch_mode nil)) 

(not (setf plan_cycle nil) ) 

(not (setf plan_state nil) ) 

(not (setf limit_leg nil) ) 
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(not (setf reduce_speed nil) ) 

(not (setf front_legs nil) ) 

(not (setf middle_legs nil) ) 

(not (setf rear__legs nil) ) 

(not (setf decision nil) ) 

(asserta plan_cycle 1) 

(asserta plan_state ' place_legs_in_the_air) ) 
t) 

(t nil) ) ) 



( de f un my_l oop ( ) 

(process -a llow-schedule) 
(cond ( (and (get_command) 
(plan) 
(execute) ) 



t) 

(t nil) ) ) 



(defun get_command () 

(cond (t (read_joystick) ) 
(t nil) ) ) 



(defun plan () 

(cond ( (and (ditch_mode) 
(ditch_plan) ) 
t) 

( (norma l_jplan) 
t) 

(t nil))) 



(defun ditch_mode () 

(cond ((match ditch_mode 'in) 
t) 

( (and (at_ditch_area) 

(asserta ditch_mode ' in) ) 
t) 

(t nil) ) ) 



(defun execute () 

(cond ( (and (execute_motion) 
(draw_robot ) ) 
t ) 

(t nil) ) ) 



(defun execute_motion () 

(cond (t (execute_planned_motion) t) 
(t nil) ) ) 



(defun draw__robot ( ) 

(cond (t (graphical_display) t) 
(t nil) ) ) 
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Normal Plan 



; normal_plan update_robot_state, check_tkm_limit , 

; leg_ plan r body_plan, generate_decision, ! . 

; update_robot — state X is update_robot_status . 

; check_tkm_limit A_leg is at_tkm_limit , A_leg \== nil, 

; assert a (limit_leg (A_leg, lift) ) . 

; check_tkm_limit . 

; leg^plan lift_a_leg. 

; leg_plan : - exchange_legs . 

; leg_plan stable. 

; leg_plan : - place_a_leg. 

; leg_plan : - wait_for_legs . 

; stable Condition is stable_p, Condition == t. 

; lift_a_leg stable, A_leg is smallest_tkm_leg, A_leg \== nil, 

; Condition is stable_without (A_leg) , Condition == t, 

; asserta (decision (A_leg,_, lift ) ) . 

; exchange_legs stable, LegA is smallest_tkm_leg, LegA \== nil, 

; LegB is max_sm_leg (LegA) , LegB \== nil, 

; Condition is has_more_tkm (LegB, LegA) , 

; Condition == t, 

; asserta (decision (LegA, LegB, exchange) ) . 

; place_a_leg A_leg is max_sm_leg (_) , A_leg \== nil, 

; asserta (decision (A_leg,_, place) ) . 

; wait_f or_legs try_new_f oothold. 

; wait_f or_legs recovery, asserta ( reduce_speed) . 

; wait__f or__legs asserta ( reduce_speed) , restore_limit_leg. 

; try_new_f oothold A_leg is leg_with_new_f oothold, A_leg \== nil, 

; asserta (decision (A_leg,_, place) ) . 

; recovery A_leg is do^recovery, A_leg \== nil, 

; asserta (decision (A_leg,_, place) ) , restore_limit_leg . 

; restore_limit_leg retract (limit_leg (A_leg, lift )) . 

; restore_limit_leg . 



(defun normal__plan () 

(cond ( (and (update_robot__state) 
(check tkm limit) 
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(leg_pla n ) 

(body_plan) 

(my-monitor limit_leg decision reduce_speed) 
(generate_decision) ) 
t) 

(t nil) ) ) 



(defun update_robot_state ( ) 
(cond ( (update_robot_status) 
t) 

(t nil))) 



(defun check_tkm_limit ( ) 

(let ( (leg) ) 

(cond ( (setf leg (at_tkm_limit ) ) 

(asserta limit — leg (list 'lift leg)) 
t) 

(t t) 

(t nil) ) ) ) 



(defun leg__plan() 

; OR tree becomes regular "cond" statement, 
(cond ( (lift_a_leg) t) 

( (exchange_leg) t) 

((stable) t) 

( (place_a_leg) t) 

( (wait_f or_legs ) t) 

(t nil) ) ) 



(defun stable () 

(cond ((stable__p) t) 
(t nil))) 



(defun lift_a_leg() 

(let ((leg)) 

(cond ( (and (stable) 

(setf leg (smallest_tkm_leg) ) 
(stable_without leg) 

(asserta decision (list 'lift leg))) 
t) 

(t nil) ) ) ) 



(defun exchange_leg ( ) 

(let ((lega) (legb) ) 

(cond ( (and (stable) 

(setf lega (smallest_tkm_leg) ) 

(setf legb (max__sm_leg lega) ) 

(has_more_tkm legb lega) 

(asserta decision (list 'exchange lega legb))) 
t) 
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(t nil) ) ) ) 



(defun place_a_leg ( ) 

(let ( (leg) ) 

(cond ( (and (setf leg (max_sm_leg nil) ) 

(asserta decision (list 'place leg))) 
t) 

(t nil) ) ) ) 



(defun wait_f or_legs ( ) 

; OR 

(cond ( ( try_new_f ootholds) t) 

( (recovery) (asserta reduce_speed) t) 

((asserta reduce_speed) ( restore_limit_leg) t) 
(t nil))) 



(defun try_new_f ootholds ( ) 

(let ((leg)) 

(cond ( (and (setf leg (leg_with_new_f oothold) ) 

(asserta decision (list 'place leg))) 
t ) 

(t nil) ) ) ) 

(defun recovery () 

(let ((leg)) 

(cond ( (setf leg (do_recovery) ) 

(asserta decision (list 'place leg)) 
(restore_limit_leg) 
t) 

(t nil) ) ) ) 



(defun restore_limit_leg ( ) 

; OR 

(cond ( (and (unify limit_leg leg) 

(retract limit_leg leg) ) 
t) 

(t t) 

(t nil) ) ) 



/ / 

;; Ditch Plan 
/ i 



; ditch_plan ditch_planr_done, retract (ditch_mode (in) ) , idle_cycle. 

; ditch_plan cycle_planner . 



. ★★★★*★**** 



Cycle planner 



★★★★★★★★★★★★ 
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; ditch_plan_done :- plan_cycle ( 6 ) , retract (plan_cycle ( 6 )) , asserta (plan_cycle (1) ) , 

; prepare_next_ditch_plan . 

; prepare_next_ditph_plan move. 

/ cycle_planner one_cycle_done f plan_cycle (N) , Nl is N+l, retract (plan_cycle (N) ) , ass 
ta (plan_cycle (Nl ) ) , 

; idle_cycle. 

; cycle_planner plan_cycle. 



.**★*★**★** Plan Cycle dispatcher ************ 



; one_cycle_done pianist ate ( one_plan_cycle_done) , retract (plan_state (one_plan_cycle_c 
e) ) , 

; initialize_plan_state . 



; plan_cycle 
_decision, ! . 
; plan_cycle 
_decision, ! . 
; plan_cycle 
_decision, ! . 
; plan_cycle 
_decision, ! . 
; plan_cycle 
_decision, ! . 



plan_cycle ( 1 ) , 
plan_cycle (2 ) , 
plan_cycle (3) , 
plan_cycle (4) , 
plan_cycle ( 5 ) , 



update_robot_ 


_state. 


ditch_plan_cycle_l , 


body_plan. 


genera 


update_robot_ 


_state, 


ditch_plan_cycle_2 , 


body_plan. 


genera 


update_robot_ 


_state. 


ditch_plan_cycle_3 , 


body__plan. 


genera 


update_robot_ 


_state, 


ditch_plan_cycle_4 , 


body_plan. 


genera 


update_robot_ 


_state. 


ditch_plan_cycle_5 , 


body__plan. 


genera 



; idle_cycle 



update_robot_state, body_plan, generate_decision, ! . 



(defun ditch_plan () 

(cond ( (and (ditch_plan_done) 

(retract ditch_mode 
(idle__cycle) ) 



( (cycle_planner ) 
t) 

(t nil) ) ) 



'in) 



.★**★★*★*** Cycle planner ************ 



(defun ditch_j?lan_done () 

(cond ((and (match plan_cycle 6) 

(retract plan__cycle 6) 

' (asserta plan_cycle 1) 

(prepare__next_ditch__plan) ) 
t) 

(t nil) ) ) 



(defun prepare_next_ditch_plan () 
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(cond ( (move) 
t) 

(t nil) ) ) 



(defun cycle_planner () 

(cond ( (and (one_cycle_done) 

(unify plan_cycle N) 

(retract plan_cycle N) 
(asserta plan_cycle (+ N 1) ) 
(idle_cycle) ) 



( (plan_cycle) 
t) 

(t nil) ) ) 



.*★*★*****★ plan Cycle Dispatcher ************ 



(defun one_cycle_done () 

(cond ((and (match plan_state 9 one_plan_cycle_done) 

(retract plan_state ' one_plan_cycle_done) 
(initialize_plan_state) ) 



t) 

(t nil) ) ) 



(defun plan_cycle () 

(cond ( (and (match plan_cycle 1) 
(update_robot_state) 
(ditch_plan_cycle_l) 
(body_plan) 

(my-monitor plan_cycle 
(generate_decision) ) 



t) 

( (and (match plan_cycle 2) 
(update_robot_state) 
(ditch_plan_cycle_2) 
(body__plan) 

(my-monitor plan^cycle 
(generate_decision) ) 
t) 

( (and (match plan_cycle 3) 
(update_robot_state) 
(ditch_plan_cycle_3) 
(body_plan) 

(my-monitor plan_cycle 
(generate_decision) ) 
t) 

( (and (match plan_cycle 4) 
(update_robot_state) 
(ditch_plan_cycle_4) 
(body_plan) 

(my-monitor plan_cycle 
(generate_decision) ) 
t) 

( (and (match plan_cycle 5) 
(update robot state) 



plan_state decision reduce__speed) 



plan_state decision reduce_speed) 



plan_state decision reduce_speed) 



plan_state decision reduce_speed) 
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(ditch_plan_cycle_5) 

(body_plan) 

(ray-monitor plan_cycle plan_state decision reduce_speed) 
(generate_decision) ) 
t) 

(t nil) ) ) 



(defun idle_cycle () 

(cond ( (and (update__robot_state) 

(body_plan) 

(my-monitor plan__cycle plan_state decision reduce_speed) 
(generate_decision) ) 
t) 

(t nil) ) ) 



. ********** 



cycles 



********* 



; initialize__plan_state asserta (plan_state (start) ) . 



; ditch_plan_cycle_l 
place_legs_in_the_air) ) 

; ditch__plan_cycle_l 
; ditch__plan_cycle__l 
; ditch__plan_cycle_l 
; ditch__plan_cycle__l 
; ditch_plan_cycle_l 
; ditch__plan_cycle_l 



plan_state (start) , retract (plan_state (start) ) , asserta (plan_stat 

place_legs_in_the_air (back_middle__legs) . 
place_legs_in_the_air (back_middle_legs) . 
back__middle__legs (f orward_rear_legs) . 
f orward_rear_legs ( f orward_middle_legs ) . 
f orward_jniddle__legs ( f orward_f ront_legs) . 
f orward_f ront_legs ( lif t_middle_legs_and_jnove) . 
lif t_middle_ legs_and__move (one_plan_cycle_done) . 



; ditch_plan_cycle_2 
back_middle_legs) ) , 

/ 

; ditch_plan_cycle_2 
/ ditch_plan_cycle_2 
; ditch_plan_cycle_2 



plan_state (start) , retract (plan_state (start) ) , asserta (plan_stat 

back_middle_legs (f orward_rear_legs) . 
back_middle_legs ( f orward_rear_legs) . 
forward_rear_legs ( f orward__middle__legs) . 
f orward_middle_legs (one_plan_cycle_done) . 



; ditch__plan_cycle_3 plan__state (start) , retract (plan_state (start) ) , asserta (plan_stat 
move_f orward_f ront_legs) ) , 

; move_forward_f ront_legs (move_f orward_jniddle_legs) . 

; ditch_plc.n_cycle_3 move_f orward_f ront_legs (move_back_middle_legs) . 

; ditch_plan_cycle_3 move_back_middle_legs (move_forward_rear_legs) . 

; ditch_j>lan_cycle_3 move_ f orward_rear__ legs (one_plan_cycle_done) . 

t 

; ditch_plan_cycle_4 : - plan_state (start ) , retract (plan_state ( start )) , asserta (plan_stat 
move_forward_middle_legs) ) , 

; move_f orward_middle_legs (one_plan_cycle_done) . 

; ditch_plan_cycle_4 move_f orward_middle_ legs (one_plan_cycle_done) . 



; ditch_plan_cycle_5 plan_state ( start ) , retract (plan__state (start )) , 
move_f orward_f ront_legs ) ) , 



asserta (plan_stat 
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ditch__plan_cycle_5 
di t c h_j3 1 a n_c y c 1 e_5 
ditch_plan_cycle_5 



move_forward_front_legs (move_f orward_midale_legs ) . 
move_forward_front__legs (move_back_middle_legs ) . 
move_back_middle_legs (move_forward_rear_legs) . 
move_forward_rear_legs (one__plan_cycle_done) . 



*************** Cycles ****** ********* ★ 



(defun initialize__plan_state () 

(cond ( (asserta plan_state 'start) 
t) 

(t nil) ) ) 



(defun ditch_jplan__cycle__l () 

(cond ((and (match plan_state 'start) 

(retract plan_state 'start) 

(asserta plan_state ' place_legs_in_the_air) 
(place_legs_ in_the_air ' back_middle_legs) ) 
t) 

( (place_legs_in_the_air ' back_middle_legs) 

t ) 

( (back_middle_legs ' forwarder ear_legs ) 
t) 

( (f orward_rear_legs ' f orward__middle_legs ) 
t) 

( (f orward_middle_legs ' forward_f ront_legs) 
t) 

( ( f orward_f ront_legs ' lif t_middle_legs_and__move) 
t) 

( (lif t_mi dd 1 e_l e g s_a ndjno ve ' one_j}lan_cycle_done) 
t) 

(t nil) ) ) 



(defun ditch_plan_cycle_2 ( ) 

(cond ((and (match plan_state 'start) 

(retract plan_state 'start) 

(asserta plan_state ' back_middle_legs) 
(back_middle_legs ' f orward_rear_legs ) ) 
t) 

( (back__middle_legs ' f orward_rear_legs) 
t) 

{ (forward_rear_legs ' f orward__middle_legs) 
t) 

( (f orward_middle__legs ' one__plan_cycle_done) 
t) 

(t nil) ) ) 



(defun ditch^plan_cycle_3 () 

(cond ((and (match plan_state 'start) 

(retract plan_state 'start) 

(asserta plan_state ' move_f orward_f ront_legs ) 
(move_f orward^f ront^legs 'move_back_middle_legs) ) 
t) 

( (move_forward_f ront_legs ' move_back_middle_legs ) 
t) 

( (move_back__middle_legs 'move_forward_rear_legs) 
t ) 

( (move_forward_rear_legs ' one_plan_cycle_done) 
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t) 

(t nil))) 



(defun ditch_plan_cycle_4 () 

(cond ((and (match plan__state 'start) 

(retract plan_state 'start) 

(asserta plan_state ' move_f orward_middle_legs ) 
(move_ f orward__middle_legs ' one_plan_cvcle_done) ) 
t) 

( (move_f orward_middle_legs ' one_plan_cycle_done) 
t) 

(t nil) ) ) 



(defun ditch_j3lan_cycle_5 () 

(cond ((and (match plan_state 'start) 

(retract plan_state 'start) 

(asserta plan_state ' move_f orward_f ront_legs ) 
(move_forward_front_legs ' move_back_middle_legs ) ) 
t) 

( (move_f orward_f ront_legs ' move_back_middle_legs ) 
t) 

( (move_back_middle_legs 'move_forward_rear_legs) 
t) 

( (move_forward_rear_legs ' one_plan_cycle_done) 
t) 

(t nil) ) ) 



•★★★★★★★★★★★★★★ States *****★★★★★★★★★* 



; back_middle_legs (Next_State) 

e (Next_State) ) , 

/ 

; back_middle_legs (Next_State) 



plan_state (back_middle_legs) , back_middle_legs_done, 
retract (plan_state (back_middle_legs ) ) , asserta (plan_st 

stop . 

plan_state (back_middle_legs ) , do_back_middle_legs , 
stop. 



; f orward_f ront_legs (Next_State) 
ne, 

tate (Next_State) ) , 

; < 

; forward_front_legs (Next_State) 



plan_state (forward_f ront_legs) ) , f orward_f ront_legs_ 
retract (plan_state ( f orward_f ront_legs ) , asserta (plan 
stop. 

plan_state (forward_f ront_legs) ) , do_f orward_f ront_le 
stop . 



; forward_middle_legs (Next_State) :- plan_state (f orward_middle_legs ) , f orward_middle_lec 
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done, 

_state (Next_State) ) , 

; f orward_middle_legs (Next_State) 
gs. 



retract (plan_state ( f orward_middle_legs ) , asserts ( 
stop . 

plan_state (f orward_middle_legs ) , do_f orward_middl 
stop . 



; forward_rear_legs (Next_State) 
te (Next_State) ) , 

; f orward_rear_legs (Next_State) 



plan_state (forward_rear_legs) , forward_rear_legs_do 
retract (plan_state ( forward__rear_legs) , asserta (plan 

stop . 

plan_state (forward_rear_legs) , do_f orward_rear_legs 
stop . 



; lif t_middle_legs_and_move (Next_State) 
e, stop, 

asserta (plan_state (Next_State) ) . 

; lif t_middle_legs_and_move (Next_State) 
middle_legs , move . 



plan_state ( lif t_middle_legs_and_move) , move 
retract (plan_state ( lif t_middle_legs_and_mov 
plan_state (lif t_middle_legs_and_move) , do__l 



; move_bac)c_middle_legs (Next_State) 
_legs_done, 

(plan_state (Next_State) ) . 

; move_bac)c_middle_legs (Next_State) 
dle_legs . 



plan_state (move_back_middle_legs ) , mo ve_b a c k_mi 
retract (plan_state (move_bac)c_middle_legs ) ) , ass 
plan_state (move_bacJc_middle_legs ) , do_move_back 



; move_f orward_f ront_legs (Next_State) 
_f r ont_legs_done , 

erta (plan_state (Next_State) ) . 

; move_forward_front_legs (Next_State) 
ard_f ront_legs . 



plan_state (move_forward_front_legs) , move_for 
retract (plan_state (move_f orward_f ront_legs ) ) , 
plan_state (move_forward_f ront_legs ) , do_move_ 



; move_forward_middle_legs (Next_State) 
rd_middle_legs_done , 

t 

serta (plan_state (Next_$tate) ) . 

; move_forward_middle_legs (Next_State) 
rward_jniddle_legs . 



pianist ate (move_forward_middle_legs) , move_f 
retract (pianist ate (move_f orward_f ront_legs) ) 
plan_state (move_f orward__middle_legs) , do__mov 



; move_forward_rear_legs (Next_State) 
iddle_legs_done , 

ta (plan_state (Next_State) ) . 

; mo ve_f orwa rd_rea r_legs (Next_State) 
d_rear_legs . 



plan_state (mo ve_f o rwa rd_re a r_legs ) , move_forwa 
retract (plan_state (move_f o rwa rd_rea r_legs ) ) , a 
plan_st ate (move_f orwa rd_rear_legs ) , do_move_fo 
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; place_legs_in_the_air (Next_State) 
e_air_done, 

f 

(plan_state (Next_state) ) , 

; place_legs_in_the_air (Next_State) 
_the_air, stop. 



plan_state (place_legs_in_the_air) , place_legs_in_ 
retract (plan_state (place_legs_in_the_air) ) , asser 
stop . 

plan_state (place_legs_in_the_air ) , do__place_legs_ 



.************** states *************** 



(defun backjmiddle_legs (next_state) 

(cond ((and (match plan_state ' back_middle_legs ) 
(back_middle_legs_done) 

(retract pianist ate ' back_middle_legs ) 
(asserta plan_state next_state) 

(stop) ) 
t ) 

((and (match plan_state ' back_middle_legs) 
(do_back_middle_legs) 

(stop) ) 
t) 

(t nil) ) ) 



(defun forward_f ront_legs (next_state) 

(cond ((and (match plan_state ' f orward_f ront_legs) 

(f orward_f ront_legs_done) 

(retract plan_state ' forward_front_legs) 
(asserta plan_state next_state) 

(stop) ) 
t ) 

((and (match plan_state ' f orward_f ront_legs) 
(do_f orward_f ront_legs) 

(stop) ) 
t) 

(t nil) ) ) 



(defun f orward_middle_legs (next_state) 

(cond ((and (match plan_state ' forward_middle_legs) 
(forward_middle_legs_done) 

(retract plap_state ' f orward_middle_legs ) 
(asserta plan_state next_state) 

- (stop) ) 
t) 

((and (match plan_state ' f orward_middle_legs) 

( do_f orwa rd_middle_legs ) 

(stop) ) 
t) 

(t nil) ) ) 
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(defun forward_rear_legs (next_state) 

(cond ((and (match plan_state f forward_rear_legs) 

( forward_rear_legs_done) 

(retract plan_state ' forward_rear_legs) 
(asserta plan_state next_state) 

(stop) ) 
t) 

((and (match plan_state ' f orward_rear_legs) 
(do_f orward_rear_legs) 

(stop) ) 
t) 

(t nil) ) ) 



(defun lift_middle_legs_ and_move (next_state) 

(cond ((and (match plan_state ' lif t_middle_legs_and_move) 
(move_done) 

(stop) 

(retract plan_state 9 lift_middle_legs_and_move) 
(asserta plan_state next_state) ) 
t) 

((and (match plan_state 9 lif t_middle_legs_and_move ) 

( lift_middle_legs) 

(move) ) 
t) 

(t nil) ) ) 



(defun move_back_middle_legs (next_state) 

(cond ((and (match plan_state 9 move_back_middle_legs) 
(move_back_middle_legs_done ) 

(retract pianist ate 9 move_back_middle_legs ) 
(asserta plan_state next_state) ) 
t) 

((and (match plan_state 9 move_back_middle__legs ) 
(do_move_back_middle_legs ) ) 
t) 

(t nil) ) ) 



(defun move_forward_f ront_legs (next_state) 

(cond ((and (match plan_^tate 9 move_f orward_f ront_legs ) 
(move_f orward_f ront_legs_done ) 

- (retract plan_state ' move_f orward_f ront_legs) 
(asserta plan_state next_state) ) 
t) 

( (and (match plan_state 9 move_f orward_f ront_legs ) 
(do_move_forward_f ront_legs) ) 
t) 

(t nil) ) ) 
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(defun move_f orward_middle_legs (next_state) 

(cond ((and (match plan_state ' move_f orward__middle_legs) 
(move_f orward_middle_legs__done) 

(retract pianist ate ' move_f orward__middle_legs) 
(asserta plan_state next_state) ) 
t) 

((and (match plan_state ' move_f orward_middle_legs) 
(do_move_f orward_middle_legs ) ) 
t) 

(t nil) ) ) 



(defun move_forward_rear_legs (next_state) 

(cond ((and (match plan_state 9 move__forward_rear_legs) 
(mo ve_f o rwa rd_r ea r_legs_done ) 

(retract plan_state 9 move_f orward_rear_legs) 
(asserta plan_state next_state) ) 
t) 

((and (match plan_state ' move_f orward_rear_legs) 
(do_move_forward_rear_legs) ) 
t) 

(t nil))) 



(defun place_legs_in_the_air (next_state) 

(cond ((and (match plan_state ' place_legs_in_the__air) 

( p 1 a ce__l egs_in_t he_a i r_done ) 

(retract plan_state ' place_legs_in_the_air ) 
(asserta plan_state next_state) 

(stop) ) 
t) 

((and (match plan_state 9 place_legs_in_the_air ) 
(do_place_legs_in_the_air) 

(stop) ) 
t) 

(t nil) ) ) 



;★★**★*★***★***** State Executors ******** ************ 



; move__back_middle_legs_done all_middle_legs_lifted, all_middle_legs_placed, 

; clearjmiddle_lifted__memory, clear_move_memory, stop. 

; do__move_back_middle_legs all_middle_legs_lifted, move_done, stop, place_middle_leg 
ack . 

; do_move_back_middle_legs :- all_middle_legs_lifted, move. 

; do__move_back__middle_legs lif t_middle_legs, stop. 
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; rnove_f o rward_f r ont_legs_done : - all__f ront_legs_lif ted, all_f ront_legs_jplaced, 

; clear_f ront_lif ted_memory, clear_move_memory, stop. 

; do_move_f orward_f ront^legs all_f ront_legs_lif ted, move_done , stop, place_f ront_lec 

; do_move_f orward__f ront_legs all_f ront_legs_lif ted, move. 

; do_move_f orward_f ront_legs lif t_f ront_legs, stop. 



; move_f orward__middle_legs_done all_middle_legs_lif ted, all_middle_legs_jplaced, 

; clear_middle_lif ted_memory, c le a r_mo ve_memo ry , stop. 

; do_move_forward_middle_legs all_middle_legs_lif ted, move_done, stop, place_middle_ 

s . 

; do_move_f orward_middle_legs all__middle_legs_lif ted, move. 

; do_move_forward__middle_legs lif t_middle_legs , stop. 



; move_forward_rear_legs_done : - all_rear_legs_lif ted, all_rear_legs_placed, 

; * clear_rear_lifted_memory, c le a r_mo ve_memo ry , stop. 

; do_move_forward_rear_legs all_rear_legs_lif ted, move_done, stop, place_rear_legs . 

; do_move_forward_rear_legs all_rear_legs_lif ted, move. 

; do_mo ve_f or wa rd_r e a r_legs lift_rear__legs, stop. 



; move asserta ( resume_movement ) . 

; stop asserta (stop_movement ) . 



; c 1 e a r_mo ve_memo r y retract (move (done) ) . 

; c 1 e a r_move_memo r y . 



; move_done 
; move_done 
; move done 



: - move (done) . 

X is at_tkm_limit , X \“ nil, asserta (move (done ) ) . 

: - X is at_stability_limit , X \== nil, asserta (move (done) ) . 



; back_middle_legs_done all_middle_legs_lifted, all_middle_legs_placed, clear_middl( 
fted_memory, clear_move_memory . 

s 

; do_back_middle_legs all_middle_legs_lif ted, place_middle_legs_back . 

; do_back_middle_legs lif t__middle_legs . 

/ 

/ all_middle_legs_lifted middle_legs (lifted) . 

; all_middle_legs_lifted X is both_middle_legs_lifted, X == t, asserta (middle_legs (] 
ed) ) . ( 

; al^middle^legs^ placed X is both_middle_legs_placed, X “ t. 

/ 

; clear_middle_lif ted_memory retract (middle_legs (lif ted) ) . 

7 

; place_middle_legs_back A_leg is placable_middle_leg, A_leg \== nil, asserta (decisi 
A_leg,_, place_back) ) . 

; place__middle_legs_back . 
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; lif t_middle_legs A_leg is lif table_middle_leg, A_leg \== nil, asserts (decision (A le 
lift) ) . 

; lif t_middle_legs . 



; f orward_f ront_legs_done all_f ront_legs__lif ted, all_f ront_legs_placed, clear_f ront_l 

tedjnemory, clear_move_memory . 



do_f orward_f ront_legs all_f ront_legs__lif ted, place_f ront_legs . 

do_forward_f ront_legs lif t_f ront_legs . 



all_f ront_legs_lif ted 
all_f ront__legs_lif ted 



front_legs (lifted) . 

X is both_f ront_legs_lif ted, X == t, asserta (f ront_legs (lif te 



/ all_f ront_legs_placed 



X is both_f ront_leg3_ placed, X == t . 



; clear_f ront_lif ted_memory retract (front_legs (lif ted) ) . 



; place_f ront_legs A_leg is placable_f ront_leg, A_leg \=- nil, asserta (decision (A__leg 
, place) ) . 

; place_f ront_legs . 



; lif t_f ront_legs A_leg is lif table_f ront_leg, A_leg \== nil, asserta (decision (A_leg, 
lift) ) . 

; lif t_f ront_legs . 



; f orward__middle_legs_done :- all_middle_legs_lif ted, all_ middle_legs_ placed, clear_midd 
_lif ted_memory, clear_move_memory . 

; do_forward_jniddle_legs all_middle_legs_lif ted, place_middle_legs . 

; do_f orward_middle_legs lif t_middle_legs . 

; place_middle_legs A_leg is placable_middle_leg, A_leg \~ nil, asserta (decision (A_l 
,_, place) ) . 

; place_middle_legs . 



; f orward_rear_legs_done all_rear_legs___lif ted, all_rear_legs_placed, clear_rear_lif te 

memory, clea r_move_memory . 

r 

; do__f orward__rear_legs all_f ront_legs_lifted, place_rear_legs . 

; do_f orward_rear_legs lif t_rear_legs . 



; all_rear_legs_lifted rear_legs (lifted) . 

; all_rear_legs_lifted X is both_rear_legs_lif ted, X == t, asserta (rear_legs (lif ted) ) 
/ 

; all__rear_legs_ placed X is both_rear_legs_placed, X == t. 

/ 

; clear_rearJ_lifted_memory retract (rear_legs (lif ted) ) . 

/ 

; place_rear_legs A_leg is placable_rear_leg, A_leg \== nil, asserta (decision (A_leg,_ 
lace) ) . 

; place_rear_legs . 



lift_rear_legs A_leg is lif table_rear_leg, A_leg \== nil, asserta (decision (A_leg,__, 
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ft) ) . 

; lift_rear_legs . 



do_lif t_middle_legs : - lif t_middle_legs . 



; place_legs_in_the_air_done X is all__legs_placed, X 

; place_legs A_leg is placable_leg, A__leg \== nil, 

; asserta (decision (A_leg, place) ) . 

; place_legs . 



★★★★★★★★★★★★★★★★ State Executors ************* ★★★★★★* 



(defun move_back_middle_legs_done () 

(cond ( (and (all_middle_legs_lif ted) 

( a 1 l_middl e_l eg s_p 1 a c ed ) 

( c 1 e a r_middl e_l i f t ed_memo r y ) 
( c 1 e a r_move_memo r y ) 

(stop) ) 
t) 

(t nil) ) ) 



(defun do_move_back_middle_legs () 
(cond ( (and (all_middle_legs_lif ted) 
(move_done) 

( p 1 a c e_mi ddl e_l e gs_b a c k ) 
(stop) ) 
t) 

( (and (all_middle_legs_lifted) 
(move) ) 
t ) 

( (and (lift_middle_legs) 

(stop) ) 
t) 

(t nil) ) ) 



t 



(defun move_f orward_f ront_legs_done () 
(cond ( (and (all_f ront_legs_lif ted) 
(all_f ront__legs_j?laced) 
(clear_f ront_lif ted_memory) 
( c 1 e a r_mo ve_memory ) 
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(stop) ) 
t) 

(t nil) ) ) 



(defun do_move_forward_front_legs () 
(cond ( (and (all_f ront_ legs_lif ted) 
(move_done) 

(place_f ront_legs) 
(stop) ) 
t ) 

( (and (all_f ront_legs_lif ted) 
(move) ) 
t ) 

( (and (lif t_f ront_legs) 

(stop) ) 
t) 

(t nil) ) ) 



(defun move_f orward_middle_legs_done () 
(cond ( (and (all_middle_legs_lif ted) 
(all_middle_legs_ placed) 

( c 1 e a r_mi ddl e_l i f t e d_memo r y ) 
( c 1 e a r_mo v e_memo r y ) 

(stop) ) 
t) 

(t nil) ) ) 



(defun do_move_f orward__middle_legs () 
(cond ( (and (all_middle_legs_lif ted) 
(move_done) 
(place_middle_legs ) 
(stop) ) 
t) 

( (and (all_middle_legs_lif ted) 
(move) ) 
t) 

( (and (lif t_middle_legs) 

(stop) ) 
t) 

(t nil) ) ) 



(defun move_forward_rear_legs_done () 
(cond ( (and (all_rear_legs_lifted) 
(all_rear_legs_placed) 

“ (clear_rear_lifted_memory) 
( c 1 e a r_move_memo r y ) 

(stop) ) 
t) 

(t nil) ) ) 
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(defun do_move_forward_rear_legs () 
(cond ( (and (all_rear_legs_lif ted) 
(move_done) 
(place_rear_legs) 
(stop) ) 
t) 

( (and (all_rear_legs_lif ted) 
(move) ) 
t) 

( (and (lift_rear_legs) 

(stop) ) 
t) 

(t nil) ) ) 



(defun move () 

(cond ( (asserta resume_movement ) 
t) 

(t nil) ) ) 



(defun stop () 

(cond ( (asserta stop_movement) 
t) 

(t nil) ) ) 



(defun clear_move_memory () 
(cond ((retract move 'done) 
t) 

(t t) 

(t nil) ) ) 



(defun move_done () 

(cond ((match move 'done) 
t) 

((and (at_tkm_limit ) 

(asserta move 'done)) 
t ) 

( (and (at__stability_limit ) 
(asserta move 'done)) 
t) 

(t nil) ) ) 



( 



(defun back_jniddle_legs_done () 

(cond ( (and (all_middle_legs_lif ted) 
(all_middle_legs_placed) 
(clear_middle_lifted_memory) 
( c 1 e a r_mo ve^memo r y ) ) 
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t) 

(t nil) ) ) 



(defun do_back__middle_legs () 

(cond ( (and (all_middle_legs_lif ted) 
(place_middle_legs_back) ) 
t) 

( (lift_middle_legs) 
t) 

(t nil) ) ) 



{defun all_middle_legs__lif ted () 

(cond {{match middle__legs 'lifted) 
t) 

( (and (both_middle_legs_lifted) 

(asserta middle_legs 'lifted)) 
t) 

(t nil) ) ) 



(defun all_middle__legs_placed () 
(cond { (both_middle__legs__placed) 
t) 

(t nil) ) ) 



(defun clear_middle__lif ted_memory () 
(cond ((retract middle_legs 'lifted) 
t) 

(t nil) ) ) 



{defun place_middle_legs_back () 

(let (leg) 

(cond ( (and (setf leg (placable_middle_leg) ) 

(asserta decision (list 'place_back leg))) 
t) 

(t t) 

(t nil) ) ) ) 



(defun lif t_middle_legs () 

(let (leg) 

(cond ( (and (setf leg (liftable_middle_leg) ) 

(asserta decision (list 'lift leg))) 
t) 

(t t) 

(t nil) ) ) ) 



(defun f orwa'rd_f ront_legs_done () 

(cond ( (and (all_f ront_legs_lif ted) 
(all_f ront__legs_placed) 

( c 1 e a r_f r o n t __1 i f t e d_memo r y ) 
(clear__move_memory) ) 
t) 

(t nil) ) ) 
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(defun do_f orward_f ront_legs () 

(cond ( (and (all_f ront_legs_lif ted) 
(place__f ront__legs) ) 
t) 

( ( lif t_f ront_legs) 
t) 

(t nil) ) ) 



(defun all_f ront_legs_lif ted () 

(cond ((match front_legs 'lifted) 
t) 

( (and (both_f ront_legs_lif ted) 

(asserta front_legs 'lifted)) 
t) 

(t nil) ) ) 



(defun all_f ront_legs_placed () 
(cond ( (both_f ront_legs_placed) 
t) 

(t nil) ) ) 



(defun clear_f ront_lif ted_memory () 
(cond ((retract front_legs 'lifted) 
t) 

(t nil) ) ) 



(defun place_f ront_legs () 

(let (leg) 

(cond ( (and (setf leg (placable__f ront_leg) ) 

(asserta decision (list 'place leg))) 
t) 

(t t) 

(t nil) ) ) ) 



(defun lif t_f ront_legs () 

(let (leg) 

(cond ( (and (setf leg ( lif table_f ront_leg) ) 

(asserta decision (list 'lift leg))) 
t) 

(t t) 

(t nil) ) ) ) 



(defun f orward_middle_legs_done () 

(cond ( (and (all_middle_legs_lif ted) 

( a 1 l_mi dd 1 e_l e g s_p 1 a c e d ) 

( c 1 e a r_mi ddle_l i f t ed_memo r y ) 
( c lea r_mo ve_memo ry ) ) 
t) 

(t nil))) 
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(defun do_f orward_middle_legs () 

(cond ( (and (all_middle__legs_lif ted) 
(place_middle_legs) ) 
t) 

( (lift_middle_legs) 
t) 

(t nil) ) ) 



(defun place_middle_legs () 

(let (leg) 

(cond ( (and (setf leg (placable_middle_leg) ) 

(asserta decision (list 'place leg))) 
t) 

(t t) 

(t nil) ) ) ) 



(defun f orward_rear_legs_done () 

(cond ( (and (all__rear_legs_lif ted) 
(all_rear_legs_placed) 
(clear_rear_lif ted_memory) 
(clear_move__memory) ) 



t) 

(t nil) ) ) 



(defun do_forward_rear_legs () 

(cond ( (and (all_rear_legs_lif ted) 
(place_rear_legs) ) 
t ) 

( (lift_rear_legs) 
t ) 

(t nil) ) ) 



(defun all_rear_legs_lifted () 

(cond ((match rear_legs 'lifted) 
t) 

( (and (both_rear_legs_lifted) 

(asserta rear_legs 'lifted)) 
t) 

(t nil) ) ) 



(defun all_rear_legs__placed () 
(cond ( (both_rear_legs_placed) 

t ) i 

(t nil) ) ) 



(defun clear_rear_lif ted_memory () 
(cond ((retract rear_legs 'lifted) 
t) 

(t nil) ) ) 
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(defun place_rear_legs () 

(let (leg) 

(cond ( (and (setf leg (placable_rear_leg) ) 

(asserta decision (list 'place leg))) 
t) 

(t t) 

(t nil) ) ) ) 



(defun lif t__rear_legs () 

(let (leg) 

(cond ((and (setf leg (lif table_rear__leg) ) 

(asserta decision (list 'lift leg))) 
t) 

' (t t) 

(t nil) ) ) ) 



(defun do_lif t_middle_legs () 
(cond ( (lift_middle_legs) 
t) 

(t nil) ) ) 



(defun place_legs_in_the_air_done () 
(cond ( (all_legs_placed) 
t) 

(t nil) ) ) 



(defun do_place_legs_in_the_air ( ) 

(let (leg) 

(cond ( (and (setf leg (placable_leg) ) 

(asserta decision (list 'place leg))) 
t) 

(t t) 

(t nil)))) 



t 
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/ 

; Plan Libraries 

.******************************************************★*** 

; body_plan speed_j^lan, tra jectory_plan . 

; speed_j?lan retract ( reduce_speed) , slow_down. 

; speed_j?lan speed_up. 

; speed_up X is speed_up_robot . 

; slow-down X is slow_down_robot . 

; t ra jectory_plan stable_m, restore_tra jectory . 

; tra jectory__plan modify_tra jectory . 

; stable_m Condition is stable_p_m. Condition *== t . 

; restore_tra jectory X is restore_command . 

; modify_tra jectory : - X is modify_command. 

; generate_decision retract (decision (A_leg, B_leg, A_decision) ) , 

; X is send_decision (A_leg, B_leg, A_decision) , fail. 

; generate_decision retract (limit_leg (A_leg, A_decision) ) , 

; X is send_decision (A_leg, A_decision) , fail. 

; generate_decision . 



(defun body_plan() 

(cond ( (and (speed_jplan) 

(tra jectory_plan) ) 
t) 

(t nil) ) ) 



(defun speed_j?lan ( ) 

(cond ( (and (retract reduce_speed) 
(slow-down) ) 
t) 

( (and (retract stopjnovement ) 
(stop__motion) ) 
t) 

( (and (retract resumejnovement) 
( r e s ume_mo t i on ) 

(speed_up) ) 
t) 

( (speed_up) t) 

(t nil))) 



(defun speed_up() 

(cond (t ( speed_up_robot ) t) 
(t nil))) 



(defun slow down ( ) 
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(cond (t (slow_down_robot ) t) 
(t nil) ) ) 



(defun t ra jectory_j^lan ( ) 

; OR 

(cond ( (and (stable_m) 

( restore_t ra jectory) ) 
t) 

( (modify_tra jectory) t) 

(t nil) ) ) 

(defun stable_m () 

. (cond ( (stable_p_m) t) 

(t nil) ) ) 



(defun restore_tra jectory ( ) 

(cond (t (restore_command) t) 
(t nil) ) ) 

(defun modify_tra jectory ( ) 

(cond (t (modif y_command) t) 
(t nil) ) ) 



(defun generate_decision ( ) 

(cond ( (not (unify decision a-decision) ) 
nil) 

( (and (unify decision a-decision) 

(retract decision a-decision) 

(print (list (second a-decision) (third decision) (first decision))) 
(send_decision (second a-decision) (thirdd decision) (first decision) ) 
(generate_decision) ) 

t) ) ) 



{defun generate_decision ( ) 

(cond ( (and decision 
(not 

(dolist (a-decision decision) (send-one-decision a-decision) ) ) 
; dolist returns nil 
(not (setf decision nil) ) 
nil) ; this simulates fail 
t) 

( (and limit_leg 

(unify limit_leg decisionl) 

(send-one-decision decisionl) 

(retract limit_leg '?) 
nil) 
t) 

(t t) 

(t nil) ) ) 



(defun send-one-decision (decision) 

; format (decision legl leg2) 

; lisp function 

(cond ((equal (first decision) 'exchange) 

(send_decision (second decision) (third decision) (first decision) ) ) 
(t 

(send_declsion (second decision) nil (first decision)))) 



t) 
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... Mode : Common-Lisp; Base: 10 -*- 






; sensor flavor definition 
/ 

(defflavor sensor (state owner) 

0 

: initable-instance-variables ) 



; contact-sensor flavor definition 



(defflavor contact-sensor ( ) 
(sensor) 

: initable-instance-variables ) 



(defmethod (contact-sensor :initti) 
(leg-name) 

(setf state (send self -.sensing))) 



(defmethod (sensor :contact-p) 
() 

state) 



(defmethod (sensor -.sensing) 

0 

; simulation purpose 
; graph-terrain is object. 

(setf state 

(let* ( (leg-pos-wrt-body (send (send owner :executor) 

: leg-pos-wrt-body) ) 

(leg-pos-wrt-earth 

(to-earth-transf orm (send owner :get-Hl) leg-pos-wrt-body) ) 
(x-y-pos (list (first leg-pos-wrt-earth) 

(second leg-pos-wrt-earth) ) ) 

(leg-height (third leg-pos-wrt-earth) ) ) 

(if (< leg-height ( + 0.07 (send graph-terrain :get-height x-y-pos))) 
t 

nil) ) ) ) 



i 
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;;; Mode : Common- Lisp; Base: 10 - * - 

• ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★nr* 



; stability-calculator flavor definition 
/ 

(deff lavor stability-calculator (safety-margin 

saf ety-margin-p 
large -safety-margin 
large -saf ety-margin-p 
recovery- vector 
recove ry-vector-p 
owner) 

() 

: in it able -instance -variables) 



(defmethod (stability-calculator rinitti) 

0 

(setf safety-margin 0.4) 

(setf safety-margin-p 0.2) 

(setf large-safety-margin 0.5) 

(setf large-safety-margin-p 0.4) 

(setf recovery-vector ' (0 0 0) ) 

(setf recovery-vector-p ' (0 0 0))) 



(defmethod (stability-calculator : get-recovery-vector ) 

0 

recovery- vector) 



(defmethod (stability-calculator : get-recovery-vector-p) 
() 

recovery-vector-p) 



(defmethod (stability-calculator : convert-to-recovery-vector) 
(stability- vector) 

(let ( (sm (first stability-vector)) 

(vect (second stability-vector) ) ) 

(cond ( (< sm 0) 
nil) 

( (< sm 0.1) 

(magvect (/ 1 sm) vect)) 

(t 

(magvect (/ 0.1 (* sm sm) ) vect))))) 



(defmethod (stability-calculator :mo re-stable) 
(supporting-legs H legl leg2) 

(let ((stabilityl (send self : calculate-stability 

f (cons legl 

(stability2 (send self rcalculate-stability 

(cons leg2 



(if (> stabilityl stability2) 
t 

nil) ) ) 



supporting- legs) 
supporting- legs) 



H) ) 
H) ) ) 
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(defmethod (stability-calculator :stable-m) 

; predict H <= H10 

(supporting-legs H) 

(let ( (stability-vector 

(send self : get-stability supporting-legs H) ) ) 

(cond ( (>= (first stability-vector) 
large- safety-margin) 
t) 

(t 

(if (>- (first stability-vector) safety-margin) 

(setf recovery-vector 

(send self : convert-to-recovery-vector stability-vector)) 
(setf recovery-vector '(0 0 0))) 
nil)))) 



(defmethod (stability-calculator :stable-p-m) 

; present H <= HI 

(supporting-p-legs H) 

(let* ( (stability-vector 

(send self : get-stability supporting-p-legs H) ) 

(st-margin (first stability-vector) ) ) 

(cond ( (>= st-margin 

large-saf ety-margin-p) 
t) 

(t 

(setf recovery-vector-p 

(send self : convert-to-recovery-vector stability-vector) ) 
(if (< st-margin saf ety-margin-p) 

(my-print (list 'st-p st-margin))) 
nil) ) ) ) 



(defmethod (stability-calculator ratable) 
(supporting-legs H10) 

(if (>= (send self : calculate-stability 

supporting- legs 

safety-margin) 



t 

nil) ) 



H10 ) 



(defmethod (stability-calculator :stable-p) 
(supporting-p-legs HI) 

(if (>= (send self : calculate-stability 

supporting-p-legs HI) 

saf ety-margin-p) 
t 

nil) ) 



! 

(defmethod (stability-calculator : calculate-stability) 
(supporting-legs H) 

(first (send self : get-stability supporting-legs H) ) ) 



(defmethod (stability-calculator : get-stability) 



stability-t2 . lisp 



Thu Nov 29 11:35:01 1990 



3 



(supporting-legs H) 

(if (>= (counting supporting-legs) 3) 

(measure-distance (center-of -gravity H) 
(convex-hull 

(supporting -points 
supporting-legs) ) ) 

' (- 100.0 (0 0 0 ) ) ) ) 



(defun convex-hull (points) 

; returns clockwise-ordered 
; point list of convex hull 
(reverse 

(convexl (car points) points 
' (0 0 0) nil) ) ) 



(defun convexl (current-point points previous-pt visited-pts) 
(let* ( (min-out-pt (min-out current-point previous-pt points)) 
(pos (position min-out-pt visited-pts :test #'equal))) 
(cond (pos 

(subseq visited-pts 0 ( + pos 1))) 

(t (convexl min-out-pt 
points 

current -point 

(cons min-out-pt visited-pts)))))) 



(defun min-out (current-pt pv-pt pts) 

(let* ( (min-pt nil) 

(min-angle 100) 

(angle 0) ) 

(dolist (a-pt pts) 

(cond ( (not (equal a-pt current-pt) ) 

(setf angle (turning-angle (vectsub current-pt pv-pt) 

(vectsub a-pt current-pt) ) ) 

(cond ( (< angle min-angle) 

(setf min-pt a-pt) 

(setf min-angle angle)))))) 

min-pt) ) 



(defun turning-angle (vect new-vect) 

; 2 D space clock-wise turning angle 
; Neither vect should not be zero vector. 

(let* ( (vectl-0 (list (first vect) (second vect) 0)) 

(vect2-0 (list (first new-vect) (second new-vect) 0) ) 
(normal-vect (crossprod vectl-0 vect2-0)) 

(polarity (> (thifd normal-vect) 0)) 

(value (/ (dotprod vectl-0 vect2-0) 

(* (magnitude vectl-0) 

(magnitude vect2-0)))) 

(angle 0) ) 

(if (>= value 1) 

(setf value 1) ) 

(if (<= value -1) 

(setf value -1) ) 



stability-t2 .lisp 



Thu Nov 29 11:35:01 1990 



4 



(setf angle (acos value) ) 
(if polarity 

(- (* 2 pi) angle) 
angle) ) ) 



(defun center-of-gravity (H) 

; center-of-body is represented wrt earth coordinate, 
(let ( (x (aref H 0 3) ) 

(y (aref H 1 3) ) ) 

(list x y ) ) ) 

; center-of-body can be changed in future. 



(defun find-slope (first-point second-point) 

(let ( (del-x (- (car second-point) (car first-point))) 
(del-y (- (cadr second-point) (cadr first-point) ) ) ) 
(if (> (abs del-x) 0.0000001) 

(/ del-y del-x) . 
nil))) 



(defun infinite-case (x a-line) 

(list x 

(+ (* (car a-line) x) (cadr a-line)))) 



(defun intersection-point (a-line b-line) 

; Returns list (x y) . Line is list (slope crossing-point-of-axis) . 

(cond { (null (car a-line) ) (infinite-case (cadr a-line) b-line) ) 

( (null (car b-line) ) (infinite-case (cadr b-line) a-line) ) 

(t (normal-case a-line b-line) ) ) ) 



(defun in-side-of-convex-hull (center-point first-points second-points) 

(do* ( (first-points first-points (cdr first-points) ) 

(second-points second-points (cdr second-points) ) 

(in-side-flag T) ) 

( (null first-points) in-side-flag) 

(if (test-out-side (car first-points) center-point (car second-points) ) 
(setf in-side-flag nil) ) ) ) 



(defun line (slope point) 

(if slope * 

(list slope (- (second point) (* slope (first point)))) 

(list slope (first point) ) ) ) 

; When slope is infinitive, return with x-axis crossing point instead of 
; y-axis crossing point. 



(defun measure-distance (center-point convex-points) 
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; convex-points is a list of points 
; point is a list (x y z) . 

(let* ( (first-points convex-points) 

(second-points (append (cdr convex-points) 

(list (car first-points) ) ) ) ) 

(if (in-side-of-convex-hull center-point first-points second-points) 
(start-measure center-point first-points second-points) 

' (- 10.0 (0 0 0 ) ) ) ) ) 

; center-of-gravity is out-side of support pattern 



(defun normal-case (a-line b-line) 
(let* ( (al (car a-line) ) 

(bl (cadr a-line) ) 

(a2 (car b-line) ) 

(b2 (cadr b-line) ) 

(x (/ (- bl b2) (- a2 al) ) ) 

(y (+ (* al x) bl) ) ) 

(list x y) ) ) 



(defun point-distance (center-point first-point second-point) 

; returns distance and vector between cross-pt and center-pt 
(let* ( (slopel (find-slope first-point second-point) ) 

(slope2 (right-angle slopel) ) 

(cross-pt (intersection-point (line slopel first-point) 

(line slope2 center-point) ) ) 
(del-vect (vectsub center-point cross-pt ) ) 

(distance (magnitude del-vect) ) ) 

(list distance (list (first del-vect) (second del-vect) 0.0)))) 



(defun right-angle (slope) 

(cond ((null slope) 0.0) ; infinitive input slope 

( (< (abs slope) 0.0000001) nil) ; zerop slope 
(t (/ (- 1) slope) ) ) ) 



(defun start-measure (center-point first-points second-points) 

(do* ( (first-points first-points (cdr f irst-points) ) 

(second-points second-points (cdr second-points) ) 

(min-distance 10000.0 min-distance) ; infinte dummy number 10000.0 
(min-direction nil) (dis-dir nil) ) 

((null first-points) (list min-distance min-direction)) 

(setf dis-dir (point -distance center-point 

(car first-points) (car second-points) ) ) 
(cond ( (< (first dis-dir) min-distance) 

(setf min-distance (first dis-dir) ) 

(setf min-direction (second dis-dir)))))) 



(defun supporting-points (legs) 
(mapcar #' (lambda (leg) 

(send leg -.foothold)) 
legs) ) 
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(defun test-out-side (first-point second-point third-point) 

(let* ((a (- (cadr first-point) (cadr third-point))) 

(b (- (car third-point) (car first-point))) 

(c (- (+ (* a (car third-point)) (* b (cadr third-point))))) 
(decision (+ (* a (car second-point)) 

(* b (cadr second-point) ) 
c) ) ) 

(if (>= decision 0.0) 

T 

nil) ) ) 



t 



i 
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;;; Mode : Common -Lisp; Base: 10 -*- 

; stop-body definition 
/ 

• ★★★★★★★★★**★★★★*★★★★★*★*******★*★***•**★*★•***★★★**★★★*★★★•*■★*■•*••*★★•**** 



(defflavor stop-body (stop-body-motion-flag) 
(body) 

: init able -instance- variables) 



(defmethod (stop-body rafter :initti) 

0 

(setf stop-body-motion-flag nil) ) 



(defmethod (stop-body : stop-body-motion) 

0 

(setf stop-body-motion-flag t) ) 



(defmethod (stop-body : restore-body-motion) 

0 

(setf stop-body-motion-flag nil) ) 



(defmethod (stop-body : calculate-motion) 

(joystick-command legs) 

(setf joy-command joystick-command) 

(cond ( (equal support-plane-clock 10) 

(setf estimated-support-plane 

(send support-plane-estimator :get-plane legs)) 

(setf support -plane-clock 0) ) ) 

(setf support -plane-clock (+ support-plane-clock 1) ) 

(cond 

( (or stop-motion-flag stop-body-motion-flag (null modify-vector-p) ) 
(send body-controller :control 
'( 000 ) 



0 estimated-support-plane) ) 

(modify-vector-p 
(send body-controller : control 

(vectadd joy-command (send self : get-modif y-vector-p) ) 
deceleration-factor estimated-support -plane) ) 

(t 



(control body-controller 

(vectadd joy-command (send self :get-modif y-vector ) ) 
deceleration-factor estimated-support-plane) ) ) ) 
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;;; -*- Mode : Common-Lisp ; Base: 10 - x - 
; support-plane-estimator flavor definition 



(def flavor 
) 



support-plane-estimator (owner) 

0 



(defmethod 

) 



(support -plane-estimator 

0 



: initti) 



(defmethod (support-plane-estimator : get-plane) 

(legs) 

(let* ( (footholds-for-estimation (get-footholds legs)) 

(constants (get-constants footholds-for-estimation) ) ) 
(make-plane-f rom-coef ficient constants) ) ) 



• *******★*'*★*******★*★***★★★★*★**★*★★*★*■**■**★★★*★★★★***★*★ 
t 

; support -plane-estimator . get -plane 
/ 

• ■*★★★★*★★*★*★*******★★*★★*★*★****•**********★★★*★★★•*★*★*★* 



(defun add-points (points) 

; returns a list (number-of-points sum-of-points ) . 
(do ( (points points (cdr points) ) 

(i 0 (+ i 1) ) 

(sum-vect ' (0 0 0) ) ) 

( (null points) (list i sum-vect) ) 

(setf sum-vect (vectadd (car points) sum-vect) ) ) ) 



(defun average-point (points) 

(let* ( (num-&-sum- rect (add-points points)) 

(number-of-points (first num-& -sum-vect ) ) 

(sum-vect (second num-& -sum-vect ) ) ) 

(if (> number-of-points 0) 

(magvect (/ 1 number-of-points) sum-vect) 

(print "Error in finding average-point of estimate plane")))) 



(defun get-aO (bar-point al) 

(let* ( (x-bar (first bar-point)) 
(z-bar (third bar-point) ) ) 
(- z-bar (* al x-bar) ) ) ) 
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(defun get-al (points bar-point common-denominator) 

; returns al which is sum in this function, 

(do* ( (points points (cdr points) ) 

(sum 0 ) 

(x nil) (x-bar (first bar-point) ) 

(z nil) (z-bar (third bar-point) ) ) 

((null points) (/ sum common-denominator)) 
(setf x (first (car points) ) ) 

(setf z (third (car points) ) ) 

(setf sum (+ sum (* (- x x-bar) (- z z-bar) ) ) ) ) ) 



(defun get-a2 (points bar-point common-denominator) 

; returns a2 which is sum in this function. 

(do* ( (points points (cdr points) ) 

(sum 0) 

(x nil) (x-bar (first bar-point) ) 

(y nil) (y-bar (second bar-point) ) ) 

( (null points) (/ sum common-denominator) ) 
(setf x (first (ca; points) ) ) 

(setf y (second (car points) ) ) 

(setf sum (+ sum (* (- x x-bar) (- y y-bar)))))) 



(defun get-a3 (bar-point a2) 

(let* ((x-bar (first bar-point)) 
(y-bar (second bar-point) ) ) 
(- y-bar (* a2 x-bar)))) 



(defun get-a4 (points aO al a2 a3) 

(let* ( (number-of-points (counting points) ) 

(yr (get-yr points a2 a3) ) 

(zr (get-zr points aO al) ) 

(yr-bar (get-yr-bar yr number-of-points)) 

(zr-bar (get-zr-bar zr number-of-points))) 

(do ( (yr yr (cdr yr) ) 

(zr zr (cdr zr) ) 

(numerator 0) (a-yr 0) (a-zr 0) 

(denominator 0)) 

( (null yr) (/ numerator denominator) ) 

(setf a-yr (first yr) ) 

(setf a-zr (first zr) ) 

(setf numerator ( + numerator (* (- a-yr yr-bar) (- a-zr zr-bar) ) ) ) 
(setf denominator (+ denominator (* (- a-yr yr-bar) (- a-yr yr-bar) )))))) 



(defun get-common-denominator (points bar-point) 

(do* ( (points points (cdr points) ) 

(sum 0) ‘ 

(x nil) 

(x-bar (first bar-point) ) ) 

( (null points) sum) 

(setf x (first (car points) ) ) 

(setf sum (+ sum (* (- x x-bar) (- x x-bar)))))) 
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(defun get-constants (points) 

(let* ((bar-point (average-point points)) 

(common-denominator (get-common-denominator points bar-point) ) 
(al (get-al points bar-point common-denominator) ) 

(a2 (get-a2 points bar-point common-denominator) ) 

(aO (get-aO bar-point al) ) 

(a3 (get-a3 bar-point a2)) 

(a4 (get-a4 points aO al a2 a3) ) ) 

(list aO al a2 a3 a4) ) ) 



(defun get-footholds (legs) 

(do* ( (legs legs (cdr legs) ) 

(footholds nil) 

(a-leg nil) ) 

( (null legs) footholds) 

(setf a-leg (car legs) ) 

(if (send a-leg : foothold) 

(setf footholds (cons (send a-leg rfoothold) footholds))))) 



(defun get-yr (points a2 a3) 

(do* ( (points points (cdr points) ) 

(yr nil) 

(x nil) 

(y nil)) 

((null points) (reverse yr) ) 

(setf x (first (car points) ) ) 

(setf y (second (car points))) 

(setf yr (cons (- y a2 (* a3 x) ) yi ) ) ) ) 



(defun get-yr-bar (yr number-of -points) 

(do ( (yr yr (cdr yr) ) 

(yr-bar 0) ) 

( (null yr) (/ yr-bar number-of-points ) ) 
(setf yr-bar ( + yr-bar (first yr) ) ) ) ) 



(defun get-zr (points aO al) 

(do* ( (points points (cdr points) ) 

(zr nil) 

(x nil) 

(z nil)) 

((null points) (reverse zr) ) 

(setf x (first (car points) ) ) 

(setf z (third (car points))) 

(setf zr (cons (- z aO (* al x) ) zr) ) ) ) 



i 

(defun get-zr-bar (zr number-of-points) 

(do ( (zr zr (cdr zr) ) 

( zr-bar 0 ) ) 

((null zr) (/ zr-bar number-of-points)) 
(setf zr-bar ( + zr-bar (first zr) ) ) ) ) 
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(defun make-plane-f rom-coef f icient 
(let* ( (aO (first constants) ) 
(al (second 
(a2 (third 
(a3 (fourth 
(a4 (fifth 
(a (- (* a 4 
(b (- a4 ) ) 

(c 1) 

(d (- (* a2 



(constants ) 



constants) ) 
constants) ) 
constants) ) 
constants) ) 
a3) al)) 



a4 ) aO)) 

(unit -normal (normalize-vector (list a 
(dis (/ d (magnitude (list a b c) ) ) ) ) 
(list unit-normal dis) ) ) 



be))) 
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;; Mode : Cordon -Lisp ; Base: 10 -*- 

******************************************************** 

terrain-regulator flavor definition 
******************************************************** 



(def flavor terrain-regulator (body-rotate-rate-x body-rotate-rate-y 

body-trans-rate-z old-body- rot at e-r at e-x 
old-body- rot ate- rat e-y old-body-trans-rate-z 
gain min-height max-height 
etal eta2 min-eta max-eta) 

(regulator) 

: initable-instance-variables) 



(defmethod (terrain-regulator :initti) 
0 

(setf gain 5) 

(setf min-eta 0.0000001) 


; 0 


degree 


(setf 


max-eta 0.4363) 


; 25 


degrees 


(setf 


min-height 4.4) 


; 4.4 


feet 


(setf 


max-height 5.4) 


; 5.4 


feet 


(setf 


etal min-eta) 


; 0 


degree 


(setf 


eta2 0.5236) 


; 30 


degree 


(setf 

(setf 

(setf 

(list 


body-rotate-rate-x 0.0) 
body-rotate-rate-y 0.0) 
body-trans-rate-z 0.0) 
body-rotate-rate-x body-rotate- 


•rate-y 


body-trans 



(defmethod (terrain-regulator : do-terrain-regulation) 

( k-gamma -de It a -he ight ) 

; k-gamma-delta-height is ((k.x k.y k.z) gamma delta-height) 

(let* ( (k (first k-gamma-delta-height)) 

(gamma (second k-gamma-delta-height) ) 

(delta-height (third k-gamma-delta-height) ) 
(body-rotate-rate-x-n (* gain (first k) gamma)) 
(body-rotate-rate-y-n ( i 
(body-trans-rate-z-n (* 

(setf body-rotate-rate-x 
(send self :limitor 

(send self : filter body-rotate-rate-x-n body-rotate-rate-x) 

0 . 1 ) ) 



gain (second k) gamma) ) 
gain delta-height) ) ) 



(setf body-rotate-rate-y 
(send self rlimitor 

(send self : filter body-rotate-rate-y-n body-rotate-rate-y) 

0.1) ) 

(setf body-trans-rate-z 

(send self -.limitor 

(send self .'filter body-trans-rate-z-n body-trans-rate-z) 

1 ))) 

(list body-rotate-rate-x body-rotate-rate-y body-trans-rate-z) ) 



(defmethod (terrain-regulator : eta-function) 
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(eta) 

(let ((slope (/ (- max-eta min-eta) (- eta2 etal) ) ) ) 

( + min-eta (* slope (- eta etal))))) 



(defmethod (terrain-regulator : get-k-gamma-by-slope ) 

(plane H) 

(let* ( (plane-rpt-body (plane-transform plane H) ) 

(height (cadr plane-rpt-body) ) 

(eta (arc-cos (third (car plane) ) ) ) 
(k-gamma-desired-height nil) ) 

(setf k-gamma-desired-height 

(cond ( (< eta etal) (send self : low-slope plane)) 

( (< eta eta2) (send self :mid-slope eta plane H) ) 
(T (send self :high-slope plane H) ) ) ) 

(list (first k-gamma-desired-height) 

(second k-gamma-desired-height) 

(- (third k-gamma-desired-height) height) ) ) ) 



(defmethod (terrain-regulator : height-f unct ion) 

(eta) 

(let ((slope (/ (- max-height min-height) (- eta2 etal)))) 

(- max-height (* slope (- eta etal))))) 



(defmethod (terrain-regulator :high-slope) 

(plane H) 

(let* ((plane-unit-normal (first plane)) 

(a (first plane-unit-normal) ) 

(b (second plane-unit-normal) ) 

(m (sqrt (+ (* a a) <* b b)))) 

(desired-eta max-eta) 

(desired-height min-height) 

(desired-body-plane (list (list (* (/ a m) (sin desired-eta)) 

(* (/ b m) (sin desired-eta) ) 

(cos desired-eta)) 0.0)) 

(desired-body-plane-in-body (plane-transform desired-body-plane H) ) 
(unit-normal-body-plane (first desired-body-plane-in-body) ) 

(al (first unit-normal-body-plane) ) 

(bl (second unit-normal-body-plane)) 

(cl (third unit-normal-body-plane) ) 

(ml (sqrt ( + (* al al) (* bl bl) ) ) ) 

(k (if (= ml 0) 

(list 000) 

(list (/ (- bl) ml) (/ al ml) 0))) 

(gamma (arc-cos cl) ) ) 

(list k gamma desired-height) ) ) 



(defmethod (.terrain-regulator rlimitor) 
(vel max-vel) 

(if (>= (abs vel) max-vel) 

(if (> vel 0) 
max-vel 
(- max-vel) ) 
vel) ) 
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(defmethod (terrain-regulator : low-slope) 

(plane) 

(let* ( (unit-normal (first plane) ) 

(a (first unit-normal) ) 

(b (second unit-normal) ) 

(c (third unit-normal) ) 

(m ( sqrt (+ (* a a) (* b b) ) ) ) 

(k.a nil) 

(k.b nil) 

(gamma (arc-cos c) ) 

(desired-height max-height) ) 

(if (= m 0.0) 

(setf k.a 0.0 k.b 0.0) 

(setf k.a (/ (- b) m) k.b (/ am))) 

(list (list k.a k.b 0.0) gamma desired-height))) 



(defmethod (terrain-regulator :mid-slope) 

(eta plane H) 

(let* ( (plane-unit-normal (first plane) ) 

(a (first plane-unit-normal) ) 

(b (second plane-unit-normal) ) 

(m (sqrt (+ (* a a) (* b b) ) ) ) 

(desired-eta (send self : eta-function eta)) 

(desired-height (send self : height-function eta)) 
(desired-body-plane (list (list (* (/ a m) (sin desired-eta) ) 

(* (/ b m) (sin desired-eta) ) 

(cos desired-eta)) 0.0)) 

(desired-body-plane-in-body (plane-transform desired-body-plane H) ) 
(unit-normal-body-plane (first desired-body-plane-in-body) ) 

(al (first unit-normal-body-plane)) 

(bl (second unit-normal-body-plane) ) 

(cl (third unit-normal-body-plane) ) 

(ml (sqrt (+ (* al al) (* bl bl) ) ) ) 

(k (if (= ml 0) 

(list 000) 

(list (/ (- bl) ml) (/ al ml) 0))) 

(gamma (arc-cos cl) ) ) 

(list k gamma desired-height) ) ) 



(defmethod (terrain-regulator : regulate) 

(estimated-support-plane H) 

(let ( (k-gamma (send self :get-k-gamma -by-slope estimated-support -plane H) 
(send self : do-terrain-regulation k-gamma) ) ) 



(defmethod (terrain-regulator : restore) 

0 

(setf body-rotate-rate-x old-body-rotate-rate-x) 

(setf body-rotate-rate-y f old-body-rotate-rate-y) 

(setf body-trans-rate-z old-body-trans-rate-z) 

(list body-rotate-rate-x body-rotate-rate-y body-trans-rate-z)) 



(defmethod (terrain-regulator :save) 

0 

(setf old-body-rotate-rate-x body-rotate-rate-x) 
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(setf old-body-rotate-rate-y body-rotate-rate-y) 

(setf old-body-trans-rate-z body-trans-rate-z) 

) 
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;; -*- Mode : Common-Lisp; Base: 10 

it****************************************************************** 

test-overlap-leg definition 

★★★★★♦★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★I*********************** 



(defflavor test-overlap-leg 
(overlap-leg) 



) 



0 



(defmethod (test-overlap-leg : change-to-back-f oothold) 

0 

(setf foothold (first foothold-list)) 

(setf tkm (first tkm-list) ) ) 
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;;; -*- Mode : Common-Lisp; Base: 10 -*- 
•**★★*****★★★★**★★★*★****★*******★★****★***★***★**★****★************ 

; test-overlap-robot definition 

.***************************************************★*************** 



(defflavor test-overlap-robot 
( overlap -robot ) 



) 



0 



(defmethod (test-overlap-robot linitti) 

0 

(send graph-asv :init-data) 

(setf vision-system (make-instance 'ditch-vision-system :owner self)) 
(send vision-system rinitti) 

(setf joystick (make-instance ' joystick) ) 

(send joystick : reset) 

(empty-queue lift-queue) 

(setf lift-flag t) 

(let ((H)) 

(setf body (make-instance 'body : owner self)) 

(setf H (send body :initti)) 

(setf legs (list 

(make -instance 
(make-instance 
(make-instance 
(make- instance 
(make -instance 
(make -instance 
) ) 

(mapcar #' (lambda 



r test-overlap-leg 
r test-overlap-leg 
r test-overlap-leg 
' test-overlap-leg 
r test-overlap-leg 
r test-overlap-leg 



: name 


' legl 


: owner 


self) 


: name 


' leg2 


: owner 


self) 


: name 


' leg3 


: owner 


self) 


: name 


' leg4 


: owner 


self) 


: name 


' leg5 


: owner 


self) 


: name 


' leg6 


: owner 


self) 



(a-leg) (send a-leg :initti H) ) legs)) 



) 



(defmethod (test -overlap-robot : send-decision) 
(legl leg2 a-decisicn) 

(cond ((equal a-decision 'exchange) 

(send legl : send-decision a-decision) 
(send leg2 : send-decision 'place) 

(send legl : send-exchange leg2) ) 

((equal a-decision 'place_back) 

(send legl : change-to-back-foothold) 
(send legl : send-decision 'place)) 

(t 

(send legl : send-decision a-decision)))) 



(defmethod (test-overlap-robot :has-more-tkm) 
(legl leg2) 

(let ( (tkml (send legl :t,km)) 

(tkm2 (send leg2 :tkm))) 

(cond ((null tkm2) 
t ) 

((null tkml) 
nil) 

( (> tkml 

tkm2) ) ) ) ) 
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(defmethod (test-overlap-robot : lif table-leg) 

(leg) 

(cond ((member leg lif t-able-legs -.test #'equal) 
leg) 

(t nil) ) ) 



(defmethod (test-overlap-robot :placable-leg) 

(leg) 

(cond ((and (member leg place-able-legs :test t'equal) 
(send leg : has-f oothold-p) ) 

leg) 

(t nil) ) ) 



(defmethod (test-overlap-robot : lif table-front-leg) 

0 

(cond ((send self : lif table-leg (first legs))) 
((send self : lif table-leg (second legs))) 

(t nil) ) ) 



(defmethod (test-overlap-robot :placable-f ront-leg) 

0 

(cond ((send self :placable-leg (first legs))) 
((send self : placable-leg (second legs))) 

(t nil))) 



(defmethod (test-overlap-robot : both-f ront-legs-placed) 

0 

; If one of front legs has not foothold, then this will fail! 

(cond ((and (member (first legs) supporting-p-legs :test t'equal) 

(member (second legs) supporting-p-legs :test t'equal)) 
t) 

(t nil) ) ) 



(defmethod (test-overlap-robot :both-f ront-legs-lifted) 

0 

(cond ((and (not (member (first legs) supporting-p-legs :test t'equal)) 

(not (member (second legs) supporting-p-legs :test t'equal))) 
t) 

(t nil))) 



i 



(defmethod (test-overlap-robot : lif table-rear-leg) 

0 

(cond ((send self : lif table-leg (fifth legs))) 
((send self : lif table-leg (sixth legs))) 

(t nil) ) ) 
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(defmethod {test-overlap-robot : placable-rear-leg) 

0 

(cond ( (send self :placable-leg (fifth legs) ) ) 

( (send self :placable-leg (sixth legs) ) ) 

(t nil))) 



(defmethod (test-overlap-robot :both-rear-legs-placed) 

0 

; If one of rear legs has not foothold, then this will fail! 

(cond ((and (member (fifth legs) supporting-p-legs :test #'equal) 
(member (sixth legs) supporting-p-legs :test #'equal)) 
t) 

(t nil))) 



(defmethod (test-overlap-robot : both-rear-legs-lif ted) 

0 

(cond ((and (not (member (fifth legs) supporting-p-legs :test #'equal)) 
(not (member (sixth legs) supporting-p-legs :test #'equal))) 
t) 

(t nil))) 



(defmethod (test-overlap-robot : lif table-middle-leg) 

0 

(cond ((send self : lif table-leg (third legs))) 
((send self : lif table-leg (fourth legs))) 

(t nil))) 



(defmethod (test-overlap-robot :placable-middle-leg) 

0 

(cond ( (send self :placable-leg (third legs) ) ) 
((send self :placable-leg (fourth legs))) 

(t nil) ) ) 



(defmethod (test-overlap-robot :both-middle-legs-placed) 

0 

; If one of middle legs has not foothold, then this will fail! 

(cond ((and (member (third legs) supporting-p-legs :test #' equal) 

(member (fourth legs) supporting-p-legs :test #' equal)) 
t ) 

(t nil) ) ) 



(defmethod (test-overlap-robot :both-middle-legs-lifted) 

0 

(cond ((and (not (member* (third legs) supporting-p-legs rtest #'equal)) 

(not (member (fourth legs) supporting-p-legs :test #' equal))) 
t) 

(t nil))) 



(defmethod (test-overlap-robot : one-placable-leg) 
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0 

(cond ( (send self : placable-front-leg) ) 

( (send self : placable-middle-leg) ) 
( (send self :placable-rear-leg) ) 

(t nil) ) ) 



(defmethod (test-overlap-robot : all-legs-placed) 

0 

(cond ( (and (send self :both-f ront-legs-placed) 
(send self :both-middle-legs-placed) 
(send self : both-rear-legs-placed) ) 



t) 

(t nil) ) ) 



(defmethod (test-overlap-robot : at-ditch-area) 

0 

(send vision-system : on-ditch-area 
(send self :get-H10) ) ) 



************************************************************ 



Prolog Interface Functions 



************************************************************ 

(defun lif table_f ront_leg () 

(send asv : lif table-front-leg) ) 



(defun placable_f ront_leg () 

(send asv :placable-f ront-leg) ) 



(defun both_f ront_legs_j?laced () 

(send asv :both-f ront-legs-placed) ) 



(defun both_f ront_legs_lif ted () 

(send asv :both-f ront-legs-lif ted) ) 



(defun liftable_rear_leg () 

(send asv : lif table-rear-rleg) ) 



(defun placable_rear_leg () 

(send asv :placable-rear-leg) ) 



(defun both_rear_legs_placed () 

(send asv :both-rear-legs-placed) ) 
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(defun both_rear_legs_lif ted () 

(send asv :both-rear-legs-lif ted) ) 



(defun lif table_middle_leg () 

(send asv : lif table-middle-leg) ) 



(defun placable_middle_leg () 

(send asv : placable-middle-leg) ) 



(defun both_middle_legs_placed () 

(send asv :both-middle-legs-placed) ) 



(defun both_middle_legs_lifted () 

(send asv : both-middle-legs-lif ted) ) 



(defun placable_leg () 

(send asv : one-placable-leg) ) 



(defun all_legs_placed () 

(send asv : all-legs-placed) ) 



(defun at_ditch_area () 

(send asv : at-ditch-area ) ) 



t 
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;; Mode : Common-Lisp; Base: 10 -*- 

******************************************************* 

tkm-calculator flavor definition 
******************************************************* 



(def flavor tkm-calculator (working-volume owner) 

0 

: initable-instance-variables) 



(defmethod (tkm-calculator :initti) 
(leg-name) 

(cond ((equal leg-name 'legl) 
(setf working-volume 





' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-8.0832) 


( (0 


0.9397 


0 . 3420) 


-2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


-3.4167) 


( (0 


0.9397 


-0.3420) 


-2.569) ) ) ) ) 


( (equal 


leg-name 'leg2) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-8.0832) 


( (0 


0.9397 


0.3420) 


2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


-3.4167) 


( (0 


0.9397 


-0.3420) 


2.569) ) ) ) ) 


( (equal 


leg-name f leg3) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


<<1 


0 


0) 


-2.2915) 


( (0 


0.9397 


0.3420) 


-2.569) ) 




( ( (0 0 1) 5.7313) 


<<1 


0 


0) 


2.2915) 


( (0 


0.9397 


-0.3420) 


-2.569))))) 


( (equal 


leg-name 'leg4) 


















(setf 


working- vo lume 
' ( ( ( (0 0 1) 3.316) 


(<1 


0 


0) 


-2.2915) 


( (0 


0.9397 


0.3420) 


2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


2.2915) 


( (0 


0.9397 


-0.3420) 


2.569) ) ) ) ) 


( (equal 


leg-name 'leg5) 


















(setf 


working- vo lume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


3.3332) 


( (0 


0.9397 


0.3420) 


-2.569) ) 




(((0 0 1) 5.7313) 


(<1 


0 


0) 


7.8332) 


( (0 


0.9397 


-0.3420) 


-2.569))))) 


( (equal 


leg-name 'leg6) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


3.3332) 


( (0 


0.9397 


0.3420) 


2.569) ) 




(((0 0 1) 5.7313) 


( (1 


0 


0) 


7.8332) 


( (0 


0.9397 


-0.3420) 


2.569) ) ) ) ) 



) 

) 



(defmethod (tkm-calculator :find-tkm) 

(a-foothold body-trans-rate body-rotate-rate) 

; a-foothold is based on body coordinate 
; returns tkm 

(let* ( (leg-vel-rpt-body 

(get-leg- velocity 

a-foothold body-trans-rate body-rotate-rate))) 
(get-tkm a-foothold leg-vel-rpt-body working-volume) ) ) 



(defun get-distance (planes velocity leg-position) 

; global function : plane-distance 
; before start, make one plane list 

(do ((planes (append (first planes) (second planes)) (cdr planes)) 
(a-tkm nil) 

(min -tkm 10000) ) 

( (null planes) min-tkm) 

(setf a-tkm (plane-distance (car planes) velocity leg-position)) 



tkm-t . lisp 



Thu Nov 29 11:36:23 1990 



2 



(if a-tkm 

(if (and (> a-tkm 0) (> min-tkm a-tkm) ) 

(setf min -t km a-tkm) ) ) ) ) 



(defun get -leg-velocity (pos-rpt-body body-trans-rate body-rotate-rate) 
; returns leg-velocity-wrt-body 

; = - ( body-trans-rate + body-rotate-rate X pos-rpt-body ) 

(vectsub ' (0 0 0) 

(vectadd body-trans-rate 

(crossprod body-rotate-rate pos-rpt-body) ) ) ) 



(defun get-tkm (leg-pos-rpt-body velocity working-volume) 

; global function : magnitude 

; outside w.v returns nil. If speed is near 0, then returns 1000.0. 

(if (in-side-volume leg-pos-rpt-body working-volume) 

(let ( (speed (magnitude velocity) ) ) 

(if (< speed 1/1000) 

1000.0 

(/ (get-distance working-volume velocity leg-pos-rpt-body) speed) ) ) 

nil) ) 



(defun in-side-volume (position planes) 

; planes ((up front left) (back right bottom)) 

(let* ((positive-planes (first planes)) 

(negative-planes (second planes) ) 

(inside-flag T) ) 

(dolist (a-plane positive-planes) 

(if (>*= (plane-normal-distance a-plane position) 0) 
(setf inside-flag nil) ) ) 

(dolist (a-plane negative-planes) 

(if (<= (plane-normal-distance a-plane position) 0) 
(setf inside-flag nil))) 
inside-flag) ) 
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... Mode : Common -Lisp; Base: 10 - * - 

.******★★★*★**************★**★★★★********■*•*■********★*****★********** 

/ 

; user interface routines 
/ 



(def var 
(defvar 
(def var 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 



♦old-terra in -f ile-name* ) 
♦new- terrain- f ile-name*) 
♦terrain- slope- type*) 
♦terrain- slope-data*) 
♦terrain- slope-angle*) 
♦terrain-type* ) 
♦obstacle-ratio*) 
♦random-seed* ) 

*ok-f lag*) 

♦screen*) 

♦screen -width*) 
♦screen-height* ) 

♦new- lisp- listener*) 
♦ditch-width*) 
♦ditch-location*) 
♦ditch-type*) 



(defun initialize-menu-variables () 
(setf *old-terrain-f ile-name* nil) 
(setf *new-terrain-f ile-name* nil) 
(setf *terrain-slope-type* 'default) 
(setf *terrain-slope-data* nil) 

(setf *terrain-slope-angle* 0) 

(setf *terrain-type* 'random) 

(setf *obstacle-ratio* '25) 

(setf *random-seed* '125) 

(setf *ok-flag* t) 

(setf *ditch-width* 6) 

(setf *ditch-location* 21) 

(setf *ditch-type * 'no-ditch)) 



(defun get-old-terrain-file-name () 

(let ( (file-names 

(mapcar #' (lambda (file) 

(list (file-namestring file) ' : documentation "Use an old terrain") 
(directory "robot : kwak . robot .terrain-data; *.*")))) 

(if file-names 

(w.menu-choose (cons ' ("new-terrain" :value nil : documentation "Create a new ter 

in") 

file-names ) 

: label "Select terrain" 

: superior *new-lisp-listener*) 

(wrmenu-choose ' ( ("new-terrain" :value nil : documentation "Create a new terrain" 
: label "Select terrain" 

: superior *new-lisp-listener*) ) ) ) 



(defun get-terrain-slope-type ( ) 

(w : choose-variable-values 

' ( (*terrain-slope-type* :menu-alist 



( ("Default" :value default) 

("Single Angle" rvalue single-angle) 
("Manual" rvalue manual) ) ) ) 
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: label "Choose terrain slope profile" 
: superior *new-lisp-listener*) 

*ter rain- slope -type*) 



(defun get-terrain-slope-angle ( ) 

(w : choose-variable-values 

' ( ( *terrain-slope-angle* : documentation "Input terrain slope angle" 

: constraint (lambda (dl d2 d3 value) 

(cond ( (> (abs value) 30) "Too steep angle") 
(t nil))))) 

:label "Input terrain slope angle" 

•.superior *new-lisp-listener*) 

*terrain-slope-angle*) 



(defun get-terrain-slope-data ( ) 

(setf *terrain-slope-data* '((15 0) (30 2))) 

(w : choose-variable-values 

' ( (*terrain-slope-data* : documentation "Input format ( (xl hi) (x2 h2) ... )" 

: constraint (lambda (dl d2 d3 value) 

(cond ((null value) "Please input slope") 
(t nil) ) ) ) ) 

: label "Input slope data" 

: superior *new-lisp-listener *) 

*te rra in-slope -data * ) 



(defun get-terrain-obstacle-type () 

(w : choose-variable-values 

' ( (*terrain-type* :menu-alist (("Random" :value random) 

("Manual" :value manual)))) 

: label "Choose type of terrain" 

: superior *new-lisp-listener*) 

*terrain-type*) 



(defun get-terrain-random-data () 

(w : choose-variable-values 

' ( ( *obstacle-ratio* constraint (lambda 

(cond 



(*random-seed* :fixnum)) 
:superior *new-lisp-listener* ) 

(list *obstacle-ratio* *random-seed* ) ) 



(dl d2 d3 value) 

((> value 90) "Too Big") 
( (< value 0) "Error") 

(t nil) ) ) ) 



(defun get-ditch-type () 

(w: choose-variable-values 

' ( (*ditch-type* :menu-alist ( ("Add Ditch" :value add-ditch) 

("No Ditch" :value no-ditch) ) ) ) 

: label "Choose ditch option" 

: superior *new-lisp-listener* ) 

*ditch-type* ) 
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(defun get-ditch-width-location () 
(w : choose -variable -values 

' ( ( *ditch-width* constraint 



(lambda (dl d2 
(cond ( (> 



(*ditch-location* constraint 



( lambda 
(cond 



isuperior *new-lisp-listener * ) 

(list *ditch-width* *ditch-location*) ) 



d3 value) 

value 7) "Too Big") 

( (< value 3) "Too Small") 
(t nil) ) ) ) 

(dl d2 d3 value) 

((> value 32) "Too Big") 

( (< value 15) "Too Small") 
(t nil))))) 



(defun user-ok() 

(setf *ok-flag* t) 

(w : choose -variable- values 
' ( ( *ok-f lag* rboolean) ) 

: label "Do you like this terrain?" 
•.superior *new-lisp-listener* ) 
*ok-f lag*) 



(defun user-file-name ( ) 

( w : choose -variable- values 

' ( ( *new-terrain-f ile-name* : string) ) 

: label "Please provide the output file name, 
.•superior *new-lisp-listener* ) 

* new-t err a in-f ile-name* ) 



(defun user-save () 

(setf *ok-flag* nil) 

(w: choose -variable- values 

' ( (*ok-flag* "Save-p" rboolean) ) 

:label "Do you want to save this terrain?" 
: superior *new-lisp-listener* ) 

*ok-f lag* ) 



(defun move-and-shape-lisp-listener ( ) 

(setf *screen* (send *terminal-io* tsuperior)) 

(setf *screen-width* (send *screen* :width) ) 

(setf *screen-height * (send *screen* :height)) 

(setf *new-lisp-listener* (make-instance ' w : lisp-listener) ) 

(send *new-lisp-listener^ : refresh) 

(send *new-lisp-listener* : set-size 

(truncate (* 1.0 *screen-width*) ) (truncate (* 0.2 *screen-height *) ) ) 
(send *new-lisp-listener* : set-position 

0 (truncate (* 0.8 *screen-height* ) ) ) 

(send *new-lisp-listener* :set-more-p nil) 

(send *new-lisp-listener* :select)) 
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(defun restore-lisp-listener ( ) 

(send *new-lisp-listener * :kill) ) 



(defun my-print (x) 

(print x *new-lisp-listener* ) ) 



(defun my-read-char-no-hang () 

(read-char-no-hang *new-lisp-listener * ) ) 



t 
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... Mode : Common-Lisp; Base:10 -*- 

. ★*★★*****★★*★★*★★★★*★★★★★*★*★★★★★★★*★*★★★★★★★**★★★****★★★**★★★★★★** 
/ 

/ 

; vision-system definition 



• **★**★****★**★***★★★★**★★*****★★*★★*★★★★★★★★★★**★★★*★*★*★★★★★***★★★ 
/ 



(defflavor vision-system (owner) 

0 

: initable-instance-variables) 



(defmethod 

) 



( vis ion-sy stem 

0 



: initti) 



(defmethod 

) 



(vision-system : scanning) 

0 



(defmethod (vision-system :permitted-cell) 
(t-cell) 

(send graph-terrain :permitted-cell t-cell) ) 



(defmethod (vision-system : terrain-point ) 
(t-cell) 

(send graph-terrain : terrain-point t-cell) ) 
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